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ABSTRACT 


This thesis addresses the problem that virtual environments (VE’s) do not possess 
a practical, intuitive, and comfortable interface that allows a user to control a virtual 
human’s movements in real-time. Such a device would give the user the feeling of being 
immersed in the virtual world, greatly expanding the usability of today’s virtual 
environments. 

The approach was to develop an interface for the upper body, since it is through this 
part of users’ anatomy that they interact most with their environment. Lower body motion 
can be more easily scripted. Implementation includes construction of a kinematic model of 
the upper body. The model is then manipulated in real-time with inputs from 
electromagnetic motion tracking sensors placed on the user. 

Research resulted in an interface that is easy to use and allows its user limited 
interaction with a VE. The device takes approximately one sixth the time to don and 
calibrate as do mechanical interfaces with similar capability. It tracks thirteen degrees of 
freedom. Upper body position is tracked, allowing the users to move through the VE. Users 
can orient their upper body and control the movements of one arm. Uncorrected position 
data from two trackers was used to generate clavicle joint angles. Difficulty in controlling 
figure motion indicates that the sensors used lack sufficient registration for this purpose. 


Therefore, the interface software uses only onentation data for computing joint angles. 
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I. INTRODUCTION 


A. MOTIVATION 


There is a growing requirement for realistic virtual environments (VE) in which 
humans can interact. VE’s offer a safe, economical and efficient means to explore or train 
in otherwise hazardous or inaccessible environments. Uses for VE’s are numerous and span 
disciplines that include engineering, science, education, entertainment, military, law 
enforcement and medicine. Recent advances in computer and motion sensor technologies 
have made it feasible to insert humans into the VE and control their movements 1n real- 
time. Presently, however, interfaces to manipulate virtual humans are not well developed 
and are only available inside the research community. An urgent need exists to provide a 
practical, intuitive, and comfortable interface that allows its human user to feel as if he is 
immersed in the virtual world. In particular, since humans largely interact with their 


environments through their hands, an acceptable interface for the upper body is critical. 


B. GOALS 


The purpose of this thesis is to use technologies currently available to provide the 
user with a practical interface for manipulating the upper body of a human icon inserted in 
a VE. The approach is to place motion sensors on the user and have the user’s movements 
tracked and then replicated in the virtual world. With this in mind, there are three major 
goals for the research of this thesis. First, the interface should be effective in driving 
realistic and reasonably accurate movement of the virtual human. For example, if the user 
touches his right shoulder with his left finger tips, the virtual human should move its joints 
in the same manner. Ideally, when the motion is complete, the joint angles of the icon match 
those of the user. Also, the finger tips of the icon should be touching its shoulder and not 


hanging 1n space some distance from the shoulder. Second, the interface should be efficient 


enough to ensure that all actions commanded occur in real-time. The action is represented 
graphically as a smooth flow of motion. An added benefit of an efficient interface concerns 
its possible future use in large scale networked VE’s where time delays are more critical. 
Third, the interface must be intuitive and easy to use. The user can quickly learn how the 
icon’s movements correlate to his own. Additionally, the system must be relatively easy to 
calibrate and the user reasonably unencumbered while wearing the sensors. Collectively, 
the success in achieving these goals is determined by whether a user can complete a 


particular set of tasks or training in the virtual world [ZYDA92]. 


C. ORGANIZATION 


Chapter II of this thesis provides background information and reviews previous 
work related to the area of interactive human interfaces for virtual environments. Chapter 
Il provides an overview of kinematics modeling and discusses the development of the 
specific kinematics model used to effect the interface. The last part of this chapter discusses 
a prototyping tool used to implement the model. The tool 1s written in C++ using OpenGL 
graphics libraries. Chapter IV contains a description of the motion tracking equipment, the 
inverse kinematics implementation, and the software used to interface the sensors with the 
prototype tool discussed previously. Chapter V provides a discussion of the development 
and implementation of the calibration routines used. Chapter VI presents results obtained 
from this research. The last chapter, Chapter VII, provides some conclusions and discusses 


recommendations for future enhancements and research. 


Il. BACKGROUND 


The speed and power of today’s computers have made it possible to create realistic 
virtual worlds that can be populated with dynamic entities portrayed in real-time. Presently, 
articulated entity motion is primarily scripted and non-interactive [PRAT95]. One of the 
greatest challenges facing researchers concerns the insertion of individual humans into 
these interactive environments [WALD95]. 

In meeting this challenge, researches are faced with several tasks. These include: 1) 
creating a model of the human body with the desired level of detail to be used in the virtual 
world, 2) defining the level of control desired and types of user inputs required to 
manipulate the model, and 3) providing the inputs to the model in an acceptable form and 
timely manner. Tasks 2 and 3 are the essence of an interface between the computer 
generated model and user. The desire is to control most, if not all, the degrees of freedom 
represented in the model. (The level of detail of the model used is discussed below and in 
Chapter IH.) An ideal form of user inputs is the user’s own natural movements. This 
provides as intuitive an interface as possible. The goal is to make the user feel as if he is the 
figure being animated, thus giving him the feel of being immersed in the virtual 
environment. The input devices are motion sensors attached to the user and the associated 
software. The remainder of this chapter discusses the modeling of humans, tracking of 


humans, and work that has been done in putting the two together in an interface. 


A. MODELING HUMANS 


First, 1t is necessary to develop an acceptable model of the human body. Modeling 
provides a means to represent the salient characteristics of a complex system such as the 
human body. The idea is that an appropriate representation of the human body (the model) 


will be used to help efficiently describe realistic human motion in three dimensional space. 


The question is, what level of detail is required in the model to effectively represent the 
human in the virtual world? This depends on the effects one wishes to achieve by inserting 
the human into the virtual environment and the techniques that can be used together with 
the model to achieve the desired results. The terms manipulation, animation, and 
simulation describe methods that use models to create visual effects. Before discussing the 


methods that have been used to model humans, a discussion of these terms is warranted. 


1. Manipulation, Animation, and Simulation 


Manipulation generally involves the movement of objects in direct response to the 
actions of the user. Manipulation is inherently real-time and control of the figure lies 
largely with the user, though some constraints in the model may prevent certain unrealistic 
movements. Historically, manipulation has been used to reposition human figures into 
various static postures [BADL93a]. 

If the goal of manipulation is to direct a movement, then the goal of animation 1s to 
describe or choreograph motion [BADL93a]. Animation generally has an artistic quality 
about it. Success is often measured in terms of how well the motion was expressed. 
Animation techniques are generally ttme consuming and executed off-line since higher 
levels of accuracy and control are desired. 

Simulation can be defined as the process where one system’s behavior (the original 
object) can be predicted or extrapolated by observing the behavior of another system (the 
model) [CANT95]. With respect to motion, simulation has been described as automated 
animation [BADL93a]. In this case, the user describes an input ahead of time and the 
system generates the motion. The input is usually a goal and possibly some rules for 
making decisions. Control of the model can occur at a higher level. The resultant motion is 
no longer entirely in the hands of the user but can be heavily impacted by definitions of the 


model. Simulation, however, can result in very realistic movements. This form of 


animation 1s unique to computers and can be a powerful tool in the field of scientific 
visualization [WATT92] and the design of feedback control systems for legged robots 
[MCMI96a]. The drawbacks, however, are that the models used are more complex and 
computations associated with them incur more overhead. 

Obviously, the distinction between manipulation, animation, and simulation is not 
black and white and none of these terms 1s mutually exclusive of the others. For example, 
by manipulating all the parts of a figure simultaneously, one can in fact animate it. Also, 
placing joint angle limits on an otherwise simple graphical model to limit manipulation 
could be considered the beginnings of a crude simulation. The terms are helpful, however, 
for understanding the relationship between a model and its primary uses. With this in mind, 


it 1S appropriate to consider modeling methods that have been used in the past. 


2. Modeling Methods 


There are several methods that can be used to model a human. These include 
graphic, kinematic, and dynamic modeling methods. Classifying a model as a given type 1s 
largely a matter of its prevalent features, although a clear distinction between types may not 
be possible or necessary. The various modeling methods can actually be thought of as 
existing at points on a line that represents a continuum of options and features. Graphic 
models would exist at the far left of the spectrum and represent the simplest form of model. 
Dynamic, or physically based models would exist at the far nght of the spectrum. Finally, 


kinematic models would exist somewhere in-between. 


a. Graphic Models 

Graphic models are often used in conjunction with animation systems and 
commercial animation packages. These systems usually provide a means of attaching 
objects to each other. The user can construct hierarchies of objects. Manipulation of a 


parent object necessarily results in movement of its child object, though there is no real 


notion of articulation. Simply put, the ongin of the child object is relative to that of the 
parent [BADL93a]. A graphic model of a human could be created by simply attaching 
various three dimensional shapes together as needed. Needless to say, numerous graphic 


models of humans have been created with varying levels of detail. 


b. Kinematic Models 

Kinematics is the science of motion which treats motion independent of the 
underlying forces that cause it [CRAI89]. It includes position, velocity and acceleration and 
relates geometric and time considerations. Kinematics models are often used in the field of 
robotics. Research in this field has resulted in two standardized and well known kinematic 
notations. These are the Danevit and Hartenberg (DH) and the Modified Danevit and 
Hartenberg (MDH) notations [DAVI93]. The kinematic model using these notations 
specifically describes physical links connected by either revolute or prismatic joints. These 
models can be highly efficient at describing articulated rigid bodies [MCMI94]. 

Kinematic computations generally fall into two categories: forward 
kinematics and inverse kinematics. In forward kinematics, the position and orientation of 
the last link (called end-effector) of a series of connected links is calculated given the joint 
angles associated with each joint in the chain. The motion of the end-effector is determined 
by the accumulation of transformations calculated from a base link down the entire series 
of links to that end effector [WATT92]. 

Inverse kinematics is generally not as straightforward as forward 
kinematics. Inverse kinematics entails calculating joint angles given the position and 
orientation of an end-effector and sometimes intermediate links. In inverse kinematics, as 
the number of links in the chain increase, the amount of link position and orientation 
information required to unambiguously determine joint angles increases. If not enough 


information is provided, the system is said to be redundant. In such a case, additional 


constraints or heuristics must be applied to the system to achieve a unique solution. 
Eremapics) of constraints include such things as energy minimization and momentum 
conservation [WATT92]. Methods for solving inverse kinematics tend to be unique to each 
specific case. 

Both forward and inverse kinematics have been used in animation. The 
animator can create a ‘kinematic skeleton’, or model, and manipulate it using either 
method. When the desired movement 1s obtained the animator can then, if required, ‘cloth’ 
the skeleton [WATT92]. The advantage of using forward kinematics to manipulate such a 
Structure is that it affords the animator more control over the figure’s positions. Its 
disadvantage is that it is counterintuitive and complicated to use in practice. It is difficult 
to specify directly all the joint angles needed to define an exact posture of a complex figure 
such as the human. Use of inverse kinematics can provide relief from having to specify all 
joint angles, though some control may be forfeit. Disadvantages of inverse kinematics 
include possible ambiguities and the complexity of computations. The resulting additional 
overhead may be critical if the application is intended to run in real-time. 

Because it offers an efficient, standardized and _ well-developed 
representation of the motion of articulated bodies, a kinematic model is used in this 
research. Chapter [II discusses the notation, methodology and details of the kinematic 


model developed as part of this research. 


c. Dynamic Models 

Dynamics is the study of motion and the forces effecting that motion. The 
dynamic model of a human would describe body position and movement largely as a result 
of the underlying forces which the neuromuscular system exerts on the body together with 


the force of gravity. A rich simulation of the human body would necessarily include some 


aspects of dynamic modeling. Human traits such as weight, strength, and balance require 
the consideration of the underlying forces that cause them. 

There are a number of methods available to simulate the dynamics of 
articulated bodies. These methods can take one of two opposing views of the world in their 
approach. 

In the first case the view of the world is one of objects and constraints. In 
this view, human limb segments are treated as individual objects and are kept in place by 
heavily weighted constraints (forces) that join them. There is no real notion of articulation. 
For complex objects, this approach can be computationally time-consuming [KOOZ83]. 

In the second case, the world can contain articulated bodies. These bodies 
maintain a tree structure with a base link just as in the kinematics approach. The effects of 
forces can be propagated down a chain of links from the base to affect the body as a whole. 
Two of the most efficient methods for achieving these computations are the Composite 
Rigid Body (CRB) method and the Articulated Body (AB) method [MCMI95]. It has been 
shown that the AB method grows in computation linearly with the number of degrees of 
freedom modeled, whereas the CRB method grows as the cube of the number of degrees of 
freedom modeled [MCMI95]. Real-time simulation of complex aanontied bodies (24 
degree of freedom (DOF) robots) has been achieved using the AB method [MCMI95, 
MCMI]196a]. By contrast, an elegant dynamic model of a human (30 DOF) that does not use 
this method has yet to achieve a real-time capability [HODG95]. This is an active area of 


research and one that could result in real-time dynamic models of humans in the near future. 


B. TRACKING HUMANS 


Tracking of humans 1s a basic requirement of almost all VE systems. The focus of 
this research is on the human upper body, exclusive of the head. Focus is on the upper body 


since humans interact with their environment largely through this portion of their anatomy. 


Routine lower body movements such as standing, walking or running can be easily scripted 
or sapualled by an appropriate stepping algonthm [GUBI74, KWAK90]. Further, head 
tracking requirements are highly application dependent and involve close correlation with 
the type of visual display being provided to the user. Systems exist and are available 
explicitly for this purpose. Interfaces for the upper body are less well developed. 

Requirements levied on a system to track the upper body will largely be driven by 
requirements to track the hands. This is due to the fact that the range and speed of motion 
of the hands is greater than that of other parts of the upper body. Tracking devices whose 
sampling rates are fast enough to track the hands will be fast enough to track other parts of 
the body. By assuming 5 Hz as the defining frequency for hand motion and requiring 20 
times oversampling for sensor noise, [DURL95] estimates a sampling rate of 100 Hz for 
tracking the human hand. It is desirable then that any device chosen to track the upper body 
have a sample rate of at least 100 Hz. 

There are several methods that can be used to track the human upper body. These 
include mechanical, electromagnetic, acoustic, optical, and inertial methods. 
Correspondingly, there is a variety of trackers either under development or offered 
commercially. The research community is in a constant search for effective three- 
dimensional motion trackers that are affordable and easy to use [WALD95]. As a result, 
measures of performance for comparing trackers have been developed and studies of 
trackers have been conducted [MEYE92, DURL95, FREY96]. Once the type of tracker to 
be used is determined, how many are needed, where they should be physically placed, and 
how they will be calibrated must be considered. The following paragraphs discuss how the 


various types of motion trackers can be compared. 


1. Measuring Motion Tracker Performance 


Determining which tracker to use requires the use of standard measures 
performance. Currently, there is a lack of agreement on performance specifications and 
how they should be measured [DURL95]. Work in this area is needed before more 
quantifiable comparisons can be made. In the mean time, [MEYE92] suggests motion 
trackers be evaluated in more general terms by the following certain key measures, namely; 
(1) resolution and accuracy, (2) responsiveness, (3) robustness, (4) registration, and (5) 
sociability. Resolution is defined by the smallest change that can be detected by the system. 
Accuracy is considered the range over which the measured quantity is correct. Accuracy 
would include a sensor’s drift; i.e., the tendency of its output to change without any change 
in input. Responsiveness is a measure of the quickness with which new information is 
provided. It is determined by sample rate, data rate, update rate and latency, with latency 
being the most critical factor. Latency is sometimes called lag. It is the delay between the 
movement of the sensed object and report of the new position and orientation. Robustness 
is a measure of the tracker’s effectiveness in the presence of noise or other signal 
interference (including shadowing) in the operating environment. Registration is the 
correspondence between a unit’s actual position and orientation and its reported position 
and orientation over the domain of the working volume. Lastly, sociability is a measure of 
how well the tracker interfaces to track multiple objects in the same environment and the 
range of operation at which it can function. Range of operation is sometimes referred to as 
working volume. 

In addition to considering these performance factors, one might consider 
availability, cost, and ease of use before actually selecting a position tracker. Certainly 
availability or cost can put a system out of reach. If the virtual reality application associated 


with the sensors is intended for general use, then it is desirable that the system be easy to 
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set up and use. Additionally, one would like the user’s movements to be unincumbered 
since a free range of motion is necessary to enhance the illusion of being immersed in the 


virtual environment. 


2. Types of Motion Trackers 
There are many different motion trackers being developed and marketed. A brief 
overview of types of trackers and their pro’s and con’s follows. This section concludes with 


a discussion of why a specific tracker type was Selected for this research. 


a. Mechanical 

Mechanical trackers measure change in position and orientation by 
physically connecting the object being tracked to a point of reference with jointed linkages. 
In the case of a human, the point of reference can either be another part of the human body 
or a fixed surface near the human. Thus, these trackers can be separated into two basic 
types, body-based (exoskeletal systems) and ground-based systems [DURL95]. Body- 
based systems are used to track the user’s joint angles or end-effector positions relative to 
some other part of the body. Ground-based systems are attached to some surface near the 
user. Generally, the user grasps an implement whose position and orientation are tracked. 

The fact that mechanical trackers are a system of physical linkages attached 
to the body or constantly held makes them cumbersome. This is an undesirable trait if the 
goal is to give the user the freedom of motion and activity associated with being totally 
immersed in a virtual environment. This feature, however, makes mechanical trackers an 
excellent choice for haptic (force-feedback) devices, since they are ngidly mounted to the 
user and/or a nearby surface [DURL95]. Also, mechanical trackers tend to be accurate, 
responsive, and robust. On the other hand, they have poor sociability [MEYE92] and can 


be difficult and time consuming to calibrate [DURL95, PRAT95]. 
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b. Electromagnetic 

Electromagnetic trackers are trackers that utilize the electromagnetic 
spectrum. Electromagnetic spectrum is defined in a narrow sense to mean radio and 
microwave frequencies. The two methods considered here include electromagnetic and 
spread-spectrum ranging techniques. 

Electromagnetic position trackers work on the principle that a magnetic 
field induces voltage in a coil. Typically, these systems comprise a transmitter and receiver. 
The emitter generates three mutually perpendicular electromagnetic fields if the coil is in 
motion or the field is changing. The receiver 1s comprised of three orthogonal coils, each 
generating its own voltage in the presence of the electromagnetic fields for a total of nine 
voltages. The voltages generated are used to deems the sensors orientation. Voltage 
strengths of the three transmitted signals are used to determine the location of the receiver 
[MEYE92]. 

Electromagnetic trackers have been commercially available for some time 
and are reasonably inexpensive and easy to use. Not surprisingly, they are by far the most 
prevalent in use today. They tend to have good accuracy in a small working space, with 
accuracy trailing off as distances from the transmitter increase. Robustness is adversely 
effected by sensitivity to ferromagnetic objects in the vicinity, with AC-based trackers 
being more susceptible than DC-based trackers. AC-based systems tend to generate eddy 
currents in metallic objects which then cause their own electromagnetic interference. 
Adding power to the transmitter to increase the working volume can increase noise. Both 
systems are adversely impacted by noise from power sources. Responsiveness is poor 
compared to other methods. Moreover, this feature adversely impacted by design 
provisions that enhance robustness, particularly in AC-based systems. In the past, use of 
noise filtering techniques caused additional lag problems [MEYE92]. Magnetic systems, 


however, are unaffected by non-ferromagnetic occlusions, a big plus. Sociability is best in 
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an environment without ferromagnetic occlusions, but limited due to a small range of 
operation. Still, these systems can be very effective at shorter ranges. 

A second method recently proposed involves use of spread-spectrum 
ranging techniques [BIBL95]. This technique uses the measured time of arrival of 
electromagnetic pulses to determine range from a set of fixed transmitters (or receivers). 
The concept is similar to that of the Global Positioning System used for navigation. A 
minimum of three fixed transmitters would be required to determine position via 
triangulation, plus a fourth transmitter to ensure time can be accurately computed by the 
receiver [LOGS92]. Transmitted signals all occupy the same wide bandwidth and utilize 
code division multiple access (CDMA) to preclude mutual interference. 

Though these systems are not currently available, it is likely that they will 
be soon. Recent developmental efforts have shown the concept to be technically feasible 
[ADVA96]. Work performed by Advanced Position Systems, Inc., under contract to the 
Naval Research Laboratory, Washington, D.C., has shown that head tracking with accuracy 
in the millimeter range is possible. Sampling was conducted at 1000 Hz. The accuracy and 
responsiveness shown by this type of system will remove many of the limitations of current 
electromagnetic systems. Additionally, if spread spectrum transmission techniques are 
used, they are very robust in the presence of noise. However, they are not immune to 
ferromagnetic occlusions. Perhaps most significantly, sociability of this technique would 
be excellent due to longer range of operation and an ability to track a large number of 


targets simultaneously {[BIBL95]. 


c. Acoustic 
Acoustic systems use ultrasonic devices to track objects via one of two 
methods. The time-of-flight (TOF) method uses the known speed of sound through air to 


calculate distances between transmitter and receiver. Once these distances are known, 
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triangulation between several receivers and one transmitter (or vice-versa) can be used to 
determine position. Onentation is determined by tracking position at three locations on the 
same object. The second method 1s called the phase-coherence (PC) method. This method 
senses phase difference between transmitted and received signals and converts a change in 
phase to a change in position. Objects that move farther than half a wavelength in one 
update period will induce tracking error. Because of this, small errors in position 
determination can result in larger errors over time [FREY 96, LIPM90]. 

Performance of these two systems can vary greatly in an open room 
environment. Phase-coherent systems enjoy many benefits over time-of-flight systems due 
to much higher data rates [MEYE92]. If the range is small, both systems offer good 
accuracy, responsiveness, and robustness. As range increases, data rates for time-of-flight 
systems decrease, cawsing responsiveness and robustness to decrease. Both systems suffer 
severe effects from occlusion. Sociability of phase-coherent systems, however, is better 
than that of time-of-flight systems due to larger working volumes [MEYE92]. 

Acoustic systems are relatively inexpensive. They offer better range of 
operation than magnetic systems but can suffer severe effects from shadowing that can 


occur between tracked body parts or other objects. 


d. Optical 

There are more types of optical trackers than any other position tracker. 
Generally, these can be broken down into those that use unstructured light (usually infrared 
or ambient light) and those that use structured light (1.e. lasers). 

Optical trackers that use unstructured light can place the sensor on the 
tracked object and emitters at fixed locations around the tracked object (inside-out systems) 
or vice-versa (outside-in). These systems more typically track objects by placing a set of 


cameras (or other light sensitive devices) at fixed locations around the operating volume. 
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Targets may be marked with actively emitting light sources (often infrared diodes) 
[MCGH79, KOOZ80] or they may be passively tracked. Triangulation is then used to 
determine position. Distinguishing common reference points in a scene between cameras 
can be a problem, especially for systems that don’t use marking. Use of a short focal length 
to enable the camera to view a larger volume causes reduced accuracy at longer ranges. 

Pattern recognition systems are image-based systems that determine 
position by comparing known patterns against sensed patterns. These systems become less 
accurate aS range increases since the virtual image size of the object decreases. 
Additionally, they require complex algorithms to interpret scene content [MEYE92]. 

Structured light systems use lasers to scan a scene. Beam forming optics are 
used to create a plane of coherent light whose reflections are then captured by a two- 
dimensional camera. The intersection of the known plane and the line of sight from the 
camera are used to determine three-dimensional coordinates. Another common method 
uses laser spot scanning of the scene. In this method either all or only portions of the scene 
may be scanned for data [DURL95]. 

Two of the most prevalent techniques that use lasers include laser radar and 
laser interferometry techniques. Laser radar works in the same way as acoustic ranging 
techniques, except that much higher data rates are possible. Laser radar techniques include 
time-of-flight and phase shift techniques. By scanning the entire scene, a three-dimensional 
picture of the scene can be generated. Laser radar techniques are more appropniate for 
longer distances than laser techniques that use triangulation [DURL95]. 

The second technique, laser interferometry, uses a steered laser beam to 
track a reflector on the object being tracked. Phase-shift ranging and angular information 
from the steered system are used to determine position. A second method uses several 
lasers to track the reflector from different fixed positions and range information only. In 


this case, the intersection of spheres whose radii are determined from the range information 
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and whose centers are located at each laser determines the location of the point being 
tracked. The problem with these techniques is that they provide only incremental 
displacement data and loss of signal via shadowing can be cause for recalibration 
[DURL95]. 

Though there are numerous types of optical tracking systems, some general 
comments about their performance can be made. Most optical systems must inherently 
trade-off between accuracy and range of operation. Additionally, all optical systems can be 
impeded by the effects of shadowing [FREY96]. To get around these problems, designers 
often develop multiple emitter/sensor architectures that are complex and expensive. 
Conversely, however, these systems can have very high resolution and accuracy, especially 
structured light systems. They tend to be very responsive due to high data rates, and so are 
well suited to real-time applications [MEYE92]. Sociability is system dependent, but can 


be limited by range of operation and shadowing considerations. 


e. Inertial 

Inertial trackers that utilize small micro-machined linear accelerometers and 
angular rate sensors can create an “artificial vestibular system” [BACH96a] for tracking 
orientation. An angular rate sensor operates by using the differential combination of the 
outputs of two vibrating linear accelerometers or by sensing “Coreolis” torques at the base 
of a vibrating tuning fork [SYSTRO]. Angular rate sensor drift is compensated for by using 
linear accelerometers together with the earth’s gravitation field to sense angular 
orientation. Since angular rate sensors are accurate for high frequencies and linear rate 
sensors are accurate for low frequencies, a cross-over filter can be used to combine the 
inputs from each. Additionally, the earth’s magnetic field may be used to compensate for 


drift in azimuth. Three of each type of accelerometer (linear and angular) orthogonally 
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onented are needed to fully describe an object’s position and orientation in space 
[FREY 96]. 

Several companies have begun manufacturing small accelerometers but 
devices small enough for use in tracking angular orientation of human body parts are just 
now becoming available [INTE96]. These systems hold much promise for future 
application to the body tracking problem [FREY96]. Larger systems have shown that 
accuracy, resolution, response, robustness and registration requirements for human body 
tracking can be met by this technology [BACH96a]. Although the technology is currently 
expensive, it 1s expected that costs will come down as devices are marketed. Perhaps the 
greatest benefit of this type of system, however, relates to its sociability. Whereas 
electromagnetic, acoustic and optic devices all require emissions from a source to track 
objects, inertial trackers are sourceless. This precludes the inevitable disastrous effects of 
occlusions and noise, with the exception that some form of wireless transmission will be 
required to pass data from the tracked object. 

Table 1 provides a summary of the position tracker capabilities discussed 
[BIBL95, DURL95, FREY96, MEYE92]. Of the types of trackers listed, inertial and 
spread-spectrum systems were not commercially available during the research of this 
thesis. Additionally, the cost and complexity of optical systems has precluded their 
consideration for use in this research. The remaining three technologies include 
mechanical, magnetic and acoustic trackers. Mechanical techniques for tracking the upper 
body have been implemented and have been shown to be cumbersome and difficult to use. 
Acoustic trackers can provide potentially excellent accuracy and resolution together with a 
greater range of operation than magnetic trackers. However, acoustic trackers can suffer 
severe effects from shadowing of one body part by another, whereas magnetic trackers do 
not. This is a key factor in selecting magnetic trackers for use in this research, despite this 


technology’s limitations. It also indicates the potential of other technologies that don’t 
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Accuracy co M: Moderate TOF: Good Excellent Good 

Resolution SS: Excellent PC: Excellent 

Responsiveness | Good M: Poor TOF: Moderate | Excellent Good 
SS: Good PC: Good 

Robustness Excellent M: Moderate TOF: Poor System Excellent 
SS: Good PC: Good Dependant 














} Registration | Registration | Unknown | |Unknown Unknown Unknown Good 
Sociability Poor M: Poor System System Excellent 

| 
Ease Ease of Use | Use Poor | Good Good Good }Good | 


—— —— M: = — — ec sive 
SS: Expensive 

Availability M: Yes 
SS: No 


M: magnetic field method 
SS: spread spectrum method 
TOF: time of flight method 
PC: phase-coherence method 


Table 1. Tracking Technologies Compared 


suffer the effects of shadowing such as spread-spectrum ranging and inertial trackers. 
These technologies have potential for providing much needed increases in accuracy, 


response and range of operation over magnetic systems available now. 


3. Motion Tracker Placement 


Once a specific motion tracker 1s skeet placement of the trackers requires 
consideration. Specification of the number and placement of trackers to be used on the 
human body depends on a number of related factors. Some of these factors as they relate to 
electromagnetic trackers are identified here, though similar considerations apply to other 


types of trackers. 
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a. Number of Motion Trackers 

In order to unambiguously specify a single ngid body’s position and 
orientation, a single six degree of freedom (DOF) tracker is required. The tracker provides 
the x, y, and z coordinates and roll, pitch and yaw of the tracker relative to some fixed frame 
of reference. Alternatively, one could use three three DOF trackers that provide x, y, and z 
position only and place them on different locations of the same body. Suppose the rigid 
body is attached to another and the attachment point provides three degrees of freedom 
(such as a ball and socket joint). If the x, y, and z coordinates of this attachment point are 
known, then either a single three DOF orientation tracker or two three DOF position 
trackers (not colinear with the attachment point) placed on the body will be sufficient to 
specify all six DOF of the body. This second example can be considered a simple 
articulated body. 

For articulated bodies, determining the required number of trackers needed 
is dependent on the DOF’s inherent in the body. As seen from the previous example, it can 
also depend on the amount and type of DOF data provided by the tracker. For now, only 
six DOF trackers will be considered. Take for example a six DOF robot arm with the end 
of its base link fixed at a known position. All linkage position and orientations can be 
specified by using one six DOF tracker that measures position and orientation and is placed 
on the end-effector. Inverse kinematics is used to determine the joint angles of the links. If 
the robot arm were to have seven DOF’s as in the human arm, then a single six DOF tracker 
at the end-effector would not be sufficient to reproduce all the joint angles associated with 
the system. In such a case an infinite number of solutions for one or more joint angles 


results. In general, for articulated bodies that are tracked by six DOF trackers, the number 


1. For a more in depth discussion of the solvability of inverse kinematics problems see [CRAI89}. 
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of trackers required can be determined by dividing the number of DOF’s in the body by six 
and adding one if any remainder results. 

The terms under specified, completely specified and directly specified are 
introduced to help provide a framework of understanding. The term under specified is used 
when the number of trackers associated with the system is not sufficient to unambiguously 
determine all the joint angles of the articulated body. In this case, inverse kinematics will 
result in an infinite set of possible linkage positions. Additional constraints on the system 
must be identified before a unique solution will result. The term completely specified is 
used when the number of trackers associated with the system is sufficient to unambiguously 
determine all joint angles of the articulated body. In robotics, the term fully specified has 
the same meaning as completely specified. The term directly specified is used when an 
articulated body is completely specified and all physical links’ joint angles can be directly 
measured. Directly measured means that data from one or more trackers on the physical 
link can be used to determine the joint angles to within a constant transformation matrix. 

In designing an interface, selection of the type of system (under, completely 
or directly specified) can have an impact on performance. For a real-time application, the 
goal is speed. Ideally, one would like to provide just enough data to completely specify the 
system while avoiding the computational overhead associated with using complex inverse 
kinematics. A completely specified system avoids the computational overhead associated 
with implementing algorithms that enforce realistic constraints on the system. A directly 
specified system ensures that only the simplest of inverse kinematics are used to determine 
all limb segment orientations. The trade-offs associated with a directly specified system 
include the cost of additional hardware and a more encumbered user. 

If it is desired to directly measure the joint angles associated with a link, 
then a single six DOF tracker attached to the link will suffice. If the x, y, and z coordinates 


of the joint attaching the link to the rest of the body are known, then a single three DOF 
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orientation tracker is all that 1s required. From this, it is easy to see that by knowing or 
tracking the position of the base link of an articulated body all that is needed to fully specify 


the system is one three DOF orientation tracker on each link. Table 2 shows the number of 


sensors required for complete and direct specification of the human arm. 


# Sensors # Sensors 
Arm Cumulative Complete Direct 
Joint Segment Joint DOF DOF? — Specification‘ 





Table 2. Joint DOF’s and Sensor Requirements [WALD95] 


a, assumes coordinates of shoulder are known 
b. elbow and wrist joint sensor numbers reflect use of six DOF sensors 
c. only three DOF orientation trackers required 

b. Physical Placement of Trackers 

In a general sense, the possible placement of trackers was alluded to in the 
previous section. If it is desired to create a directly specified system for the anticipated 
speed it offers, a six DOF tracker can be placed on the base link and three DOF orientation 
trackers on each additional link. With this information, the figure’s complete state can be 
specified. There are other considerations, however, for placing the trackers. 

There are several problems associated with using trackers to infer joint 
angles. The first concerns soft tissue and the potential for relative motion between the 
tracker and the limb segment [DURL95]. For example, suppose a tracker is placed on the 
upper arm near the shoulder to measure three DOF motion. It is unlikely that this tracker 
will sense the roll of the upper arm accurately, since the tissue it 1s attached or strapped to 
remains relatively stationary in relation to the underlying bone structure. A better place to 


sense shoulder roll would be near the elbow or possibly even on the forearm near the elbow. 
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A second problem associated with inferring joint angles involves the fact 
that human joints are not perfect hinge or spherical joints [DURL95]. In fact, their axes of 
rotation move with the joint angle. In some cases they aren’t revolute joints at all. For 
example, the forearm can be rolled yet the elbow is a single DOF hinge joint. This is 
because the bones of the forearm form a closed chain of links with two bones twisting 
around each other to allow the wrist to turn. In the first case, where human joints 
approximate revolute joints, the effects of not using a model and calibration scheme to 
account for this are within the expected resolution of today’s magnetic trackers. In the 
second case, a simplified model can still be used dependent on the level of detail desired in 
the application. It is important, however, to position the tracker where the DOF’s modeled 
can actually be sensed. 

So far, implementations which rely exclusively on three DOF position 
trackers have not been discussed. In fact, some tracking systems only provide positional 
data from which orientation must be derived. It was already shown how two position 
trackers can replace one orientation tracker on a single link. This is, however, an increase 
in the number of sensors a user must wear. AS mentioned earlier, considerations for 
placement are similar to those for the orientation tracker with the added constraint that 
sensors not be colinear or near colinear with each other and the joint. 

One suggestion has been to place one three DOF position tracker at each 
joint of the human [BIBL95]. Placing three DOF position trackers at the joints is less 
problematic with regards to soft tissue, since joints generally have less soft tissue on them. 
Problems arise, however, with regards to determining methods of attachment at the joints 
that are secure, yet unencumbering to the user. The sensors can be offset from the joints to 
mitigate these effects, though calibration of the offset will be required. This system, 


however, is not directly specified and requires more complex inverse kinematics to 
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determine the orientation of each limb. At least two additional position trackers will be 


needed on the end-effectors (hands, head, etc.) to make the system completely specified. 


4. Motion Tracker Calibration 


Once the trackers are placed on the tracked object or limb, they must be calibrated. 
The calibration process is used to determine the relationship between the tracker’s frame of 
reference and the tracked object’s frame of reference. Once the transformation matrix 
between the tracker and object is known, then data reported by the tracker can be 
transformed into a position and orientation of the tracked object in world coordinates. 

Figure 1 illustrates the basic concept. The three frames of reference (world, tracker 
and limb) are right hand coordinate systems. A black dot inside the circle indicates the third 
axis coming out of the page. An ‘X’ signifies the axis going into the page. The world 
coordinate system is fixed. For magnetic trackers it is often associated with the 


transmitter’s antenna which does not move. The tracker’s frame of reference is fixed to the 


Limb 
Segment 
Coordinate 







a 


System xy i; 1 World 
cg Coordinate 
ea ; System 
= J (transmitter) 
/ 
/ 
4 
A, 7 A “H, 
a” 
» Me 
Motion 
Tracker 
Coordinate 
System 
(receiver) 


Figure 1: Relationships Between Frames of Reference 
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case of the magnetic receiver. The limb segment’s frame of reference is fixed with respect 
to the underlying bone structure of the limb and is usually located at the joint whose 
position and angles are to be determined. If it is assumed that the tracked object is a ngid 
body, then the position and orientation of the limb relative to the frame of reference 
attached to the tracker is constant. Knowing this constant transformation matrix then makes 
it possible to transform tracker position and orientation in world coordinates to limb 


segment position and orientation in world coordinates. 
If °H;, represents a homogeneous transfer matrix for mapping coordinates given 


with reference to frame b into coordinates that are referenced to frame a [CRAI89], then 


the relationship discussed above is mathematically represented as: 

Hi) = OH TH, (eq Daly 
where tx, r and / stand for transmitter, receiver and limb segment respectively. If the limb 
segment is placed in a known position and orientation relative to the world coordinate 


system, then “H, for this position will be known. While the limb segment is still in this 
position, data from the tracker can be read to determine “H,. From these two known 


quantities the constant ’H, matrix can be calculated. This is basically what occurs during 
the calibration process. Once the world coordinates of the tracked object are known, then 
by knowing the dimensions of the limb segment it is possible to render a suitably scaled 
replication of it in the virtual environment. A more detailed analysis of calibration is 


provided in Chapter V. 


C. PREVIOUS WORK INTERFACING HUMANS 


Man-machine interfaces are necessary for the useful operation of any computer. 
Significant advances in the usability and application of computers have followed advances 


in techniques for interfacing computer users with their machines. Some examples include 
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development of higher level programming languages, mouse pointing devices, and 
graphical ei interfaces (GUIs). More recently, advances in motion tracking technology 
have made it possible to create interfaces that permit real-time control of the highly 
dynamic entities that populate virtual worlds. Without these interfaces, interaction with 
virtual environments 1s greatly restricted, reducing the scope of their application. 

In order to insert the human in the virtual environment, it is necessary to construct 
an appropriate man-machine interface. The goal is to make the interface as transparent and 
intuitive as possible for the user. The desire is to enhance the perception that the user is 
immersed in the virtual environment. At the same time, the interface must convey a large 
amount of information on the activities of the user to the machine. This is a tall order. 

In this section, a review of some of the work on interfaces designed to insert the 
human in a virtual environment is presented. These implementations combine the use of 
computer models and motion tracking hardware as previously discussed. The focus is on 


systems that interface upper body movement. 


1. Individual Soldier Mobility System 

The Individual Soldier Mobility System (ISMS) is a simulator designed to allow the 
inclusion of dismounted infantry into large-scale simulated combat exercises. Its hardware 
was developed at Sarcos, Inc. Software for ISMS was developed by groups at the Naval 
Postgraduate School, University of Utah Center for Engineering Design, Sarcos, and 
University of Pennsylvania [WALD95]. Project funding was provided by the Army 
Research Laboratory - Human Research Engineering Directorate in late 1993. The system 
was first demonstrated in 1994. This demonstration marked the first time that individual 
dismounted soldiers could operate in a virtual environment together with traditional vehicle 
simulators [BADL94]. System components include the /ndividual Portal or I-PORT, 


through which user inputs and visual/force feedback are provided, the Jack human software 
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model, and interface software. I-PORT is currently one of the few force feedback devices 
available for inserting humans into the virtual environment [PRAT94, PRAT95]. 

The structure of ISMS as it was first demonstrated 1s shown in Figure 2. The I- 
PORT system is shown in Figure 3. To use ISMS, the soldier wears a body suit that 
measures upper body joint angles. He holds an instrumented mifle and sits in a room called 
the Walk-in Synthetic Environment (WISE) on a pedal-based mobility simulator. 
Computer generated imagery is provided via Head Mounted Display (HMD) and three 
large video screens. The suit, seat, rifle and image generator together make up I-PORT. The 
soldier moves through the environment by pedaling the I-PORT. He changes his direction 
of motion by twisting in his seat. The surface normal for the soldier’s virtual position is 
computed and provided so that resistive feedback can be implemented consistent with 


terrain [GRAN9S5]. 
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The human model that is used as part of ISMS is called Jack. Jack software was 
developed over a two decade period at the University of Pennsylvania. Jack is a general 
purpose interactive environment used for manipulating articulated figures [BADL93a]. It 
uses its own notation (called Peabody) and was primarily designed for use as a human 
factors visualization tool. The human model associated with Jack is largely kinematic, 
though provision is made for associating strength values with joints and computing figure 
balance. The full Jack model includes 73 joints and 136 DOF. A general purpose constraint 
engine is provided with Jack that uses an iterative inverse kinematics procedure. High-level 


behavioral control is possible with Jack via software that manages articulation and 
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constraints when given a goal directed input. Jack provides the capability to simulate 
certain human behaviors. Not all of this functionality is needed or used in ISMS. 

I-PORT and Jack are interfaced together through Naval Postgraduate School 
Networked Virtual Environment (NPSNET) application software. This software includes 
DI_Guy and NPSNET-DI. DI_Guy provides overall control of the system and information 
flow as shown in Figure 2. Additionally, it provides an interface with the rest of the 
networked virtual environment through use of Distributed Interactive Simulation (DJS) 
protocols. To do so it creates the appropriate protocol data units (PDU’s) and sends them 
out over the net. NPSNET-DI manages terrain data, provides force feedback, and renders 
visuals and audio. The Jack model accepts peace! velocity, position and sensor suit 
inputs. It provides joint angles and position to the NPSNET applications for rendering. Jack 
provides realistic joint angles for the lower body that are in concert with inputs from the 
mobility simulator. These angles cause the lower body to be rendered as though the figure 
is Standing, walking or running. Upper body angles are taken from the sensor suit, checked 
for validity, and passed on. 

Subsequent versions of the ISMS system were redesigned to overcome significant 
inefficiencies in the original system [PRAT95]. In the original system, DI_Guy ran on a 
central server and Jack software ran on its own workstation. The new system architecture 
eliminated the need for this server and workstation, but created the need for a low level 
controller for -PORT. Most of the functionality of the original system was converted into 
linkable libraries and associated with a single main NPSNET application. In the interest of 
efficiency, Jack was replaced by Jack Motion Library (JackML), a subset of the original 
software. 

As can be seen from Figure 3, the upper body tracking in ISMS is accomplished 
through a mechanical tracker. The sensor suit of I-PORT provides accurate position and 


orientation in real-time, but is not without its problems. During ISMS’s first demonstration 
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in February, 1994 some who used it felt the suit was cumbersome and difficult to adjust. 
The ont requires a lengthy calibration process for each user. Calibration measurements 
prior to the demonstration were often accompanied by high noise levels that resulted in 
jerky motion [PRAT94]. At later demonstrations of the suit, the numbers of users made it 
impractical to calibrate the system for each user. As a result, it was decided that the icon 
lock its hands on the rifle and inverse kinematics be used to provide joint angles for the 
arms. While this was visually acceptable, the user could never put his rifle down in the 


virtual environment [PRAT94]. 


2. Minimally Sensed Humans and Jack 


At the University of Pennsylvania efforts have been undertaken to track, in real- 
time, the posture and position of the human body using a minimal number of six DOF 
sensors [BADL93b]. The goal of this research is to recreate the user’s upper body 
positioning without encumbering him with sensors. Four six DOF magnetic Flock of Birds 
trackers from Ascension Technology, Inc. are used. Flock of Birds is a DC-based system. 
Sensors are placed as shown in Figure 4, with one each on the palms, head and waist. The 
system is underspecified. Unsensed joint angles are determined using inverse kinematics 
and Jack's goal directed behavioral algorithms. 

The system utilizes an Extended Range Transmitter enabling the user to move in an 
8-10 foot hemisphere. Bird sensors are connected to a Silicon Graphics 310 VGX via 
RS232 interface operating at 38,400 baud. Special interface software is used to circumvent 
the IRIX operating system and allow Bird data to reach the application with minimal delay. 
This allows for each tracker to provide 25-30 updates per second of which only eight to ten 
are used. Overall performance in a shaded environment with 2000 polygons is eight to ten 
frames per second. This could be considered near real-time if 15 Hz is considered the 


minimal acceptable for real-time applications. The cause for this relatively slow 
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Figure 4: A Minimally Sensed Human [BADL93b] 


performance is attributed directly to the inverse kinematics routine [BADL93b]. In this 
case, Jacobian inverse kinematics as described in [KLEI83] might work better. The routine 
runs in the interframe update to strike a balance between position accuracy and refresh rate. 
It was also noted that the system requires better noise filtering algorithms, both for the 


equipment and for erratic inputs from the human operator. 


3. Complete and Direct Specification Systems 

Recently, research at the Naval Postgraduate School has focused on improving the 
usability of I-PORT by replacing its mechanical sensor suit with a magnetic system. 
Parallel efforts were undertaken to develop systems that were completely and directly 
specified [MCMI96b, WALD95]. These efforts centered on modeling and tracking the 


human arm. In each case, an MDH model of the human arm was developed to be used with 
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the JackML software of ISMS. The models were designed in such a way that inverse 
kinematics result in joint angles that are within a constant of those utilized in Jack. This 
permits the interface to quickly convert results to "Jack angles" and then override existing 
Jack angles to render the figure in the proper posture. Both systems utilize the Polhemus 
Fastrak magnetic tracking system. The Fastrak system is an AC-based system with range 
of operation similar to Flock of Birds. 

The completely specified system uses four six DOF trackers, one on the back of the 
neck, one on each wrist, and one to track a rifle. Two degrees of freedom in the wrist are 
fixed resulting in a 5 DOF arm. A software driver for the Polhemus sensors was written to 
accept data from the sensors at the highest rate available. If two sensors are utilized (1.e. 
one arm is tracked), then 60 Hz is the maximum available from the sensors. For all four 
Fastrak sensors, the rate is 30 Hz. Data was provided to the SGI workstation via RS232 at 
9600 baud. The 9600 baud rate proved to be a eomeneue preventing sampling rates higher 
than 15 Hz. Faster RS232 interfaces with the SGI should be possible, but have not yet been 
implemented [MCMI96b]. The 15 Hz sample rate, sensor noise, and overhead associated 
with communicating with NPSNET and Jack, cause somewhat jerky motion to be 
displayed. 

Work on the directly specified system remains incomplete. The system was to 
include four sensors, to be placed on the torso, upper arm, forearm and hand. Figure 5 
shows tracker placement on the arm. With this placement, all seven DOF's of the arm could 
be tracked. 

It is the purpose of this thesis to continue research described in [MCMI96b] and 


[WALD95], and extend its applicability to the entire upper body. 


ail 





Figure 5: Sensor Position on Arm [WALD95] 


D. SUMMARY 


Issues concerning the modeling and tracking of humans have been presented. An 
overview of graphic, kinematic and dynamic modeling methods was given. Considerations 
for tracking humans, including motion tracker performance metrics, advantages and 
disadvantages of various trackers, and the placement and calibration of trackers was 
discussed. The chapter concluded with a survey of previous work related to this thesis. In 
Chapter II, forward kinematics and the model developed as part of this research is 


presented. ; 
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fil. FORWARD KINEMATICS 


The interface between the actual and virtual human makes use of both forward and 
inverse kinematics. Inverse kinematics is used to determine joint angles from motion 
tracking data. Forward kinematics is used to render the graphics (in this case in OpenGL). 
In this chapter, kinematics notation, modeling of the upper body, and implementation of the 


model in OpenGL are discussed. 


A. KINEMATICS NOTATION 

As mentioned, the two standardized and well known kinematic notations are the 
Danevit and Hartenberg (DH) and the Modified Danevit and Hartenberg (MDH) notations. 
The kinematic model using either of these notations specifically describes physical links 
(rigid bodies) connected by single DOF revolute or prismatic joints. The links are 
connected serially in chains or sometimes in tree-like structures. In both notations, links 
and joints are typically numbered in succession from the base link or root outward. 
Coordinate systems are assigned to each link via a standard set of rules depending on the 
notation used. The DH and MDH notations are equivalent, with the exception that the link 
frame of reference for DH links is attached to the outboard motion axis of the link, whereas 
the link frame of reference for MDH links is attached to the inboard motion axis. Joints in 
either case are indexed based upon the frame associated with them. The MDH notation is 
described here, though either notation can be used to model the upper body. 

Figure 6 shows coordinate system assignment and the standard MDH parameters 


associated with links. The joint between link,_; and link; is labeled joint 1. The coordinate 


frame for a link is fixed to the end of the link nearest the base. The z-axis of this frame lies 
along the line of motion of the joint and the x-axis along the common normal between this 


joint axis and the next. If the motion axes of the inboard and outboard joints intersect, then 


513, 


the x-axis 1s assigned perpendicular to the two. If the motion axes of the joints are parallel 
to each other, then the x-axis lies along a common normal between them at an arbitrary 


location. 
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Figure 6: Link Coordinate System Assignment and MDH Parameters [CRAI89] 


There are four parameters needed to describe the relationship between. the frame for 
link;_; (denoted {i-1}) and the frame for link; (denoted {1}). Link;_; is referred to as the 
inboard link and link; is referred to as the outboard link. The four parameters are: 

¢ inboard link length 

a;.; = distance from Z;.; to 2; measured along xj. 
¢ inboard link twist 

QO ;-; = angle between Z;_, and Z; measured about X;_, 
¢ outboard link offset 

d; =distance from %;_, to %; measured along 7; 


¢ outboard joint angle 


©; =angle between X;_, to %; measured about 2; 
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These parameters are constant with the following exceptions: for revolute joints ©. 1S 
variable, and for prismatic joints d; is variable. The model constructed as part of this 
research only includes revolute joints (prismatic joints rarely occur in nature). | 

The transformation between {1-1} and {1} can be represented by a transformation 
matrix ‘/T;. This is found to be the result of two rotations and two translations executed as 
specified by the following equation: 

HT 5 =R,(03.1)D, (aj )R(O)D,(d;) (eq. 3-1) 

where R,(@;_;) is a rotation about the x-axis of {i-1} of a;_,degrees, and D,(a;.,) is a 
translation down the x-axis a;_; units. This equation is used for rendering links in their 
proper position. As will later be shown, the translations and rotations are executed in the 
order specified by (eq. 3.1) to move from the frame of the link rendered to the frame of the 
next link in the series. 

An equivalent expression for (eq. 3.1), found by multiplying out the transformation 


matrices of (eq. 3.1), is [CRAI89]: 


cos ©; ~sin 0; 0 Gi] 
i-] T — sin©; cos(a;__ 1; ) cosO; cos(a;_ ;) —sin(a; _ 1) —sin(a;_ 1 )d; (eq. 3.2) 
Y sin; sin(a;_,) cosO;sin(a;_ 1) cos(a;_ 1) cos(a;_ 4)d; 
0 0 0 1 


An expression for ‘T;,, can be found by calculating the inverse of (eq. 3.2) as 


follows [MCMI96b]: 
cos; sin©;cos(a;_ 1) sin; sin(a;_ 1) ~4i-1¢0SQj 
iT.,= -sin©; cos@;cos(a;_ 1) cos@;sin(a;_ 1) %-1SinOj (eq. 3.3) 
in] 0 ~sin(;_ 1) cos(a;_ 1) ~d; 
0 0 0 1 
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Having described the MDH notation and the transformations that relate one link’s 
position to the next, it is now possible to describe the upper body model that uses this 


notation. 


B. THE KINEMATIC MODEL 

The goal in creating a model of the human upper body is to provide a kinematic 
representation of the salient features of human upper body anatomy and motion. Since 
human upper body motion is defined largely by the amount of freedom of movement in the 
human skeleton, the model can be designed with emphasis on replicating this structure. 
Prominent structures include the spine, clavicles, arms and hands. The model provides only 
an approximation of the movement of these structures, balancing the requirements for 
accuracy, simplicity and speed of computation. 

Figure 7 provides a sketch of one possible approximation of the human upper body 
skeleton. The model uses multiple DOF joints attached by ngid links. The spine is 
approximated by two three DOF joints placed at the waist and mid-torso. Though the 
actual human spine is made up of numerous two DOF vertebrae, the two three DOF joints 
allow the model to bend, twist, and lean in fashion similar to an actual human. Similarly, 
the human clavicle is a complex bone structure which has been modeled by a single two 
DOF joint and its associated rigid link. This allows the model to move the shoulders up and 
down or forward and back. The human shoulder 1s a ball joint and so it is modeled with a 
three DOF joint. The human elbow can be modeled with a single DOF rotary joint as in the 
actual structure. Finally, forearm roll and the two DOFs in the wnist are modeled together 
as a three DOF joint at the wrist. This provides for the three DOF normally seen at the hand 
or end-effector. Taken together, the joints provide the entire upper body model 24 DOFs. 

It is now necessary to define the model mathematically using the MDH notation. It 


should be noted that multiple DOF joints can be modeled by chaining zero length and offset 
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Figure 7: Upper Body Model with 24 Degrees of Freedom 
links. This places the inboard and outboard axes of motion of a link at the same location. 


Therefore, the 24 DOF model requires 24 MDH links. The links in this case form two serial 


chains and are numbered starting from the base link at the waist. 


Coordinate systems are assigned to each link in accordance with the rules 
previously defined. Figure 8 shows the result of assigning coordinate systems. The z-axes 


are associated with motion axes in accordance with MDH notation convention. In defining 
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Figure 8: MDH Link Coordinate Axis 
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the MDH links, one must first decide the order in which motion axes are addressed. Once 
the order of motion axes 1s determined, there are two choices for each z-axis about which 
the motion can occur. The x-axes are also chosen in accordance with the rules previously 
described. 

Table 3 provides a listing of MDH parameters associated with the choice of axes in 
Figure 8. Link lengths and offsets have been chosen somewhat arbitrarily to reflect the 
proportions of atypical human. These lengths can be changed later when the model is sized 


to the actual user of the interface. Link twists and joint angles are defined using their MDH 


definitions and the right hand rule. The reference joint angle for each joint, © ;, 1S that joint 


angle needed to position the model as shown in Figure 3. Also shown in the table are the 
desired limits of each joint angle. These were chosen to correspond roughly to actual limits 
in human motion. 

Note that this is one of many possible ways to define a kinematic model with the 
DOFs shown in Figure 7. Different models result for a different ordering of motion axes. It 
iS Sometimes necessary to redesign a kinematic model by reordering motion axes to avoid 
singularities which can arise when computing inverse kinematics. This model has been 


shown to work well with the inverse kinematics computations which have been 


implemented here. 
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Table 3. MDH Kinematic Parameters 


a. result in positioning as shown in Figure 8 
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C. IMPLEMENTATION IN C++ 

The kinematic model was implemented in C++ using object-oriented programming 
techniques. The C++ implementation of the model (forward kinematics) was used as a 
prototyping tool to investigate inverse kinematics. Figure 9 shows the class and object 


hierarchy of the software. The C++ code can be found in Appendix A. 
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Figure 9: C++ Class and Object Hierarchy 
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The basic building block for the upper body structure is the link. A link object 
contains data members which include the four MDH parameters required to move from the 
previous link’s frame to its own frame. This is accomplished prior to rendering the link. 
This is shown in the code for the draw( ) member function of the link class in Figure 10. 
Forward kinematics are computed by setting a link’s joint angle and then executing the 
code in Figure 10. Notice that this code results in the SGI computing the result of the matrix 
multiplications listed in (eq. 3.1). Another method to compute the forward kinematics 
would be to calculate the matrix found in (eq. 3.2) and then multiply the top of the OpenGL 
model-view stack by this matrix. 

Other data members in the basic link class include the joint angle initial (or 
reference) position and the minimum and maximum allowable joint angles. Data members 
which identify parameters for drawing the link as an eight-sided diamond are also included. 
The 24 concrete link classes inherit these features, but are modified in two ways. First, their 
constructors are made to instantiate links with the parameters shown in Table 3 along with 
corresponding draw-parameters. Second, some link’s draw functionality 1s modified to 
accommodate special requirements for that link. In most cases links draw themselves along 
the x-axis using a simple drawDiamond( ) function as shown in Figure 10. In some cases 
this is not adequate. For example, the link6 class is responsible for drawing the head of the 
figure (with nose), while the link15 and link24 classes must draw the hands with thumbs 
(oriented initially along the z-axis). Notice that only links with positive lengths are drawn. 

| Links are put together in the upperbody class. The upperbody class instantiates link 
objects of each of the 24 link types. The upper body is drawn by drawing each of the links 
as Shown in Figure 11. The order in which links are drawn is important. Notice that links 7 
and 16 both share the same inboard link (link 6), so it is necessary to push and pop the 


model-view stack to draw links that form the right side of the upper body. The remaining 
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Vola Grav) 


{ 
gliTranslated ((GLdouble) inboard_link_length, 0.0, 0.0); 


glRotated ((GLdouble) inboard_twist_angle, 1.0, 0.0, 0.0); 
glTranslated (0.0, 0.0, (GLdouble) joint_displacement) ; 
glRotated ((GLdouble) joint_angle, 0.0, 0.0, 1.0); 
if (draw_length > 0.0){ 

drawDiamond(0.0, 0.0, 0.0, draw_length, draw_width, 


draw_depth, draw_offset); 


Figure 10: Link Class Draw Member Function 


functionality of the upperbody class is implemented in fashion similar to that of the 
draw( ) function. 

The body class object is made up of an upperbody object, a lowerbody object and a 
FastrakClass object. The lower body object is static, and is simply drawn as a unit in a 
single position and orientation. The FastrakClass object provides position and orientation 
data to the body so that its upper body links can be drawn in the correct position. A 
discussion of how this is done is found in Chapter TV. When rendered in it’s reference 
position, the body appears as shown in Figure 12. The body 1s shown facing into the page 
with thumbs forward and elbows directly aft. It is made of 86 polygons and serves to 
provide visual feedback for determining the suitability of kinematics and calibration 


algorithms. 


D. SUMMARY 
The chapter began with a discussion of kinematics and the Modified Danevit- 
Hartenberg notation used in this research. This was followed by an explanation of the 


kinematic model developed. The chapter concluded with a discussion of the architecture 
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void Upperbody: :draw() 
{ 


//draw upper body so he's facing into screne 
giRotated(90.0, 1.0, 020, 0.0); 
gteeweced(-90.0, 0.0,;a05G) 1.0); 
linkl.draw(); // Aaraw each link, starting at the waist 
link2.draw(); 
lpninsearaw () ; 
link4.draw(); 
linkS.draw(); 
link6é.draw(); 
glPushMatrix(); // after drawing upper torso, remember where it was 
link7 .draw() ; // start drawing left side from the shoulder 
link&.draw(); 
link9.draw() ; 
link10.draw() 
link11.draw() 
link12.draw() 
Rimicks -draw ( ) 
link14.draw() 
link15.draw() 
) // come back to the upper torso 
) // start drawing the right side from the shoulder 
) 
) 
) 
) 
) 
) 
) 
) 


glPopMatrix ( 
link16. draw ( 
linki7 .draw ( 
Jinks draw ( 
link19 . draw ( 
link20. draw ( 
link21.draw ( 
link22 .draw ( 
fink2s .draw ( 
lank24 draw ( 
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Figure 11: Upperbody Class Draw Member Function 


of the C++ code used to graphically depict the model. In Chapter IV, Polhemus tracking 
of the human body is discussed. The inverse kinematics required to determine model joint 


angles and associated software implementations are also explained. 
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Figure 12: Body Rendered in OpenGL 
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IV. INVERSE KINEMATICS 


Inverse kinematics is used to determine the joint angles which position the model. 
Polhemus Fastrak sensors are placed on the user’s body in a manner that results in a 
completely specified system. Inverse kinematic computations fall into one of two distinct 
types; those that use angle data, and those that use position data from the trackers. In this 
chapter a brief overview of the Polhemus Fastrak system hardware and software setup is 
provided. This is followed by a description of tracker placement. Finally, examples of 


angle and position tracking inverse kinematics computations are provided. 


A. POLHEMUS TRACKING 

The Polhemus 3Space Fastrak system is an electromagnetic tracker which provides 
up to six DOF tracking data [POLH93]. As previously mentioned, electromagnetic 
position trackers work on the principle that a magnetic field induces voltage in a coil. The 
system includes a transmitter and up to four receivers which can be used for tracking. The 
emitter generates three mutually perpendicular electromagnetic fields. Each receiver 
contains three orthogonal coils. Each coil generates its own voltage in the presence of the 
electromagnetic fields for a total of nine voltages. The voltages generated are used to 
determine the sensor’s orientation. Voltage strengths of the transmitted signals are used to 
determine the location of the receiver. The Fastrak system is an AC-based system, and as 
such, tends to be more susceptible to ferromagnetic interference than a DC-based system 


such as Ascension Technology’s Flock of Birds [MEYE92]. 


1. Hardware Setup and Device Driver 


The Fastrak hardware, device driver, and their implementation were nearly 


identical to that used in [MCMI96b]. A complete description of the hardware setup, device 
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driver, system initialization, and system operation can be found in [MCMI96b]. A brief 
overview along with any differences in this implementation is provided here. 

Figure 13 shows the Fastrak hardware setup. In the center of the figure is the 
Fastrak unit. The unit 1s interfaced to the SGI workstation via serial cable. A DB9 
connector for the computer’s serial port with pins connected as shown is used to allow for 
software flow control. On the back of the unit are eight DIP switches used to configure the 
unit’s serial communication. The switch setting shown specifies RS232 9600 baud 
connection with eight bits (no parity) and software flow control enabled. On the front of 
the unit, ports for the four sensors and transmitter are provided. The four DIP switches on 
this side of the unit are placed in the down position to enable all four sensors. Polhemus 
provides two types of transmitters, a standard transmitter and a “Long Ranger” which 


provides extended range (up to 15 ft.). In this case, the Long Ranger was used. 
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Figure 13: Fastrak Hardware Setup [MCMI96b] 
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The device driver can be found in the FastrakClass software in Appendix B. The 
original eition of the device driver was developed for the Isotrak by Sarcos, Inc. and later 
revised at the Naval Postgraduate School [MCMI96b]. This version was modified by 
McMillan in an attempt to develop the fastest possible interface between the Fastrak unit 
and SGI workstation. McMillan found that most of the original functionality of the driver 
was unnecessary. The previous system polled each sensor for data and could be 
reconfigured during operation. Though extremely flexible in its utility, it was very slow. 
He therefore discarded it in favor of a faster system. The result is a driver which, 1) allows 
configuration only during initialization and, 2) sets up only one communication mode, a 
mode not previously available. The mode allows continuous binary stream of data. 

Fastrak initialization occurs when the constructor of the FastrakClass is called. 
This happens when the Body class constructor is called. During the process, an ifstream 
reference to the input configuration file is passed into the FastrakClass constructor. The 
configuration file, called fastrak.dat, is found in Appendix C. It contains the parameters 
needed to specify the configuration of the Fastrak unit. During initialization, the 
constructor reads data from the configuration file, opens the I/O port, configures the 
Fastrak unit, and spawns a process which will handle the continuous stream of data. 
During configuration of the Fastrak unit, a data link between SGI and Fastrak unit is 
established. Parameters sent to the Fastrak unit from the SGI specify which receivers will 
be used along with the data types to be passed and their format. In this case, all four 
receivers provide homogeneous transformation matrices (orientation and position) which 
are passed using a 16 bit format. Parameters are also sent specifying what-frame of 
reference will be associated with the transmitter’s antennas. Figure 14 shows the frame of 
reference associated with receivers and transmitter. These frames are used in the inverse 


kinematics computations. 
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Figure 14: Coordinate System Assignment to Polhemus Devices 


During continuous operation, data 1s read in one byte at a time. There are several 
actions the device driver must accomplish to make data available to the main application 
program. The driver must first query the port for available data. It then takes the data and 
processes it by first identifying the sensor to which it belongs and then converting it to IEEE 
floating point format. The data is then placed in buffers for access by the application 
program. 

Figure 15 shows how data is processed during continuous operation. After the 
serial port is configured during initialization, a function called pollContinuously( ) is 
sproc’ed. This function calls the function getPacket() continuously until its parent process 
sends a quit signal, at which time the function is exited. The getPacket( ) function converts 
the data stream into packets of data from each sensor. It does this by reading data from the 


serial port into a temporary character buffer read_buffer. It then determines whether there 
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is enough data to complete at least one entire packet of information from a sensor. If there 
is, the eatekeets are processed and written into a second buffer datarec_buf. If not, then the 
read times out and is tried again. During the procedure, data in the stream cycles 
sequentially through the sensors and the getPacket( ) process must synchronize with it. 
This is done by identifying a special set of header bytes which specifies a particular 
sensor’s data. Data in read_buffer is discarded until getPacket( ) synchronizes with the 


data stream. 


: Application 






Device Driver get_all_inputs() 


Data Stream 


/ 
from Fastrak 
a el Serial Port 
Buffer 


read_buffer | datarec_buf | datarec 


Figure 15: Software and Buffer Organization [MCMI96b] 


In order to ensure that the application does not access data while it is being updated, 
transfer of data to the second buffer is protected by a lock and mutual exclusion. To 
minimize access to datarec_buf, the Body class function get all _inputs( ) calls the 
FastrakClass function copyBuffer( ) to copy the entire contents of datarec_buf into a third 


buffer datarec. The get_all_inputs( ) function then calls the FastrakClass function 
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getH Matrix( ) four times to acquire data for each sensor and place it into Body class data 
members. The data is then used by the Body class object to determine joint angles. 

The use of the Body class get_all_inputs( ) function is the only difference between 
this application of the device driver and the one used by McMillan [MCMI96b]. In 
McMillan’s application of the device driver a separate get_multival( ) application function 
was used to call getHMatrix( ). In both applications, get_all_inputs( ) is called at the 


beginning of each frame. 


2. Tracker Placement 


Sensor placement 1s determined by the requirement to have a completely specified 
system. Use of six instead of three DOF motion trackers results in a less encumbered user. 
The six DOF data from these trackers (position and orientation) can be used to determine 
up to Six joint angles without ambiguity if the position of the base or first joint is known 
[CRAI89]. In the case of three DOF motion trackers (orientation-only trackers), only three 
angles can be determined. This results in a requirement for a tracker to be placed on every 


limb segment, or up to twice as many trackers. 


a. Optimal Tracker Placement 

Figure 16 shows optimal positioning of the trackers for the model chosen. 
The placement is optimal because it results in the fewest number of trackers being used to 
determine all 27 DOF inherent in the model (24 joint angles plus the base position in x, y, 
and z coordinates). Notice that six trackers are required, whereas only four are available 
with the Polhemus system used. Notice also that trackers are placed on every other 
physical limb, starting from the hands inward towards the base link. This is due to the fact 
that the hands (end-effectors) must be tracked to determine their orientation. This cannot 
be done by placing a tracker on the forearm. The forearm joint angles, however, can be 


determined from data from trackers on the hand and upper arm. The upper arm tracker 
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provides information on the position of the elbow joint. This information together with 
position and orientation information from the hand sensor is adequate to determine the 
elbow angle. If the upper arm sensor is removed and placed on the clavicle to provide 
position information on the shoulder, then information from the hand sensor is required to 
determine seven joint angles between it and the shoulder. The result is an infinite number 
of possible solutions since the system is redundant. A similar logic is used to determine 


remaining placement of trackers on the upper torso and on the pelvis just below link,. The 
pelvis, or base link, is considered linkp. The tracker on the base link is the reference tracker 


for providing lower body position and orientation. All joint angles are ultimately 


determined relative to this base (frame {0}) position and orientation. 


motion tracker 





Figure 16: Optimal Motion Tracker Placement 


aye, 


b. Actual Tracker Placement 

Figure 17 shows the actual tracker placement used in this research. This 
tracker placement was chosen to investigate angles-only tracking techniques involving the 
lower waist and right shoulder, elbow and wnist. Following the angles-only investigation, 


an investigation of position tracking of the right clavicle was conducted using the torso and 


| | 
a 


upper arm trackers. 


7 ‘3 
X 
Figure 17: Actual Motion Tracker Placement 


Tracker locations and methods of attachment provide for two key 
considerations. First, tracker position is reasonably stable relative to the underlying bone 
structure throughout the range of motion. Second, the equipment is easy to don. The upper 


arm and lower arm sensors were placed near the elbow and on the wrist by means of elastic 
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velcro straps placed either directly on the arm or tightly over clothing. These locations 
provide the least amount of relative motion between tracker and underlying bone structure 
due to the muscles of the arm. The hand sensor was sewn to the back of a glove worn on 
the hand. The torso sensor was originally worn on the back using an elastic harness 
comprised of two loops through which the user placed his arms, the method used in 
[MCMI96b]. This placed the torso tracker directly between the shoulder blades and 
approximately in line with the two shoulder joints. It was found, however, that movement 
of the clavicle of either shoulder caused considerable movement of this base tracker relative 
to the back. This resulted in skewing joint angle computations. Movement was due to the 
close proximity and motion of the shoulder blades. For this reason, an alternative method 
for attaching the torso sensor was investigated. Figures 18 and 19 show sensor attachment 
and the new harness. A discussion of the effectiveness of the new harness can be found in 


Chapter VI. 





Figure 18: Polhemus Fastrak Sensor Attachment - Arm Sensors 


3. Transforming Tracker Data 


As discussed in Chapter II, the first step in using Polhemus tracker data to determine 


joint angles concerns transforming data on the tracker’s position and orientation into data 
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a) Front b) Back 
Figure 19: Polhemus Fastrak Sensor Attachment - Upper Body Harness 


on the tracked limb segment’s position and orientation. Figure 20 demonstrates how this 
is done. If the limb segment is treated as a rigid body (ie. tracker placement is such that the 
underlying bone structure moves very little in relation to the tracker) then the relationship 
between the tracker’s frame of reference and the limb’s frame of reference is constant and 


represented by the following: 

Hi iH, (eq. 4.1) 
In this case, the goal is to determine the homogeneous transformation matrix representing 
the limb’s position and orientation in world coordinates, “H),. Recall that the world 
coordinate system is attached to the transmitter’s antennas (Figure 16). ™H, is the 
homogeneous transformation matrix reported by the tracker relative to the world coordinate 


system. ‘H,is the homogeneous transformation matrix specifying the limb segment frame 


relative to the tracker’s frame and is constant. This can be found using a calibration 
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process. A more complete description of this process is contained in Chapter V. ’H) can 


then be used to transform tracker data into limb segment data using (eq. 4-1). 
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Figure 20: Relationships Between Frames of Reference 


B. INVERSE KINEMATICS 

The remaining inverse kinematics computations for determining joint angles fall 
into two categories, those that use angle or orientation information from the trackers, and 
those that use position information from the trackers. In general, orientation data is used to 
determine joint angles associated with the physical limb on which the tracker is mounted. 
Position information is used to determine joint angles associated with physical limbs which 
do not have trackers mounted on them. An example of each type of calculation is provided 


here. 


By 


1. Using Angle Data 


The first example shows how joint angles can be determined from tracker 
orientation data. In this example, tracker placement is assumed to be optimal (Figure 16). 
The actual implementation found in Appendix A uses a similar technique and is explained 
later in this chapter. 

The goal is to determine the three joint angles for the shoulder using orientation data 
from the trackers. For a tracker placed on the nght upper arm, the tracker data can be 


transformed into a homogeneous transformation matrix defining linkz9 position and 
orientation in world coordinates, or Ho. Additionally, Hyp is represented by the following: 
1 1 1 J 1 
Hq = pay OT y'T2°T 3° Ty TTT 618 17" Tyg" T 19° Tap (eq. 4.2), 
where Hpoay 1s provided by a motion tracker on the base link, or linky. The T- matrices are 


determined by using (eq. 3.2) and the parameters for each link found in Table 3. If the joint 
angles for links in the chain up to the three links associated with the tracked limb have been 


previously determined from intermediate tracker data, then 
Se = Weal et Vel eo Lig (eq. 4.3) 
is known. Rearranging (eq. 4.2) to place the known quantities on the left side of the 


equation, 

To (Hpoay) Hoo = '’T1g'®Ty9'?T29 = '’T 29 (eq. 4.4) 
where Se is the inverse of Nal ot Using the tracker data and known joint angles, a 4 x 4 
matrix of data representing TT 46 can be calculated. Again, by using (eq. 3.2) it can be 


shown that by multiplying out the T-matrices a a i and falas that mig 1S: 
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C18 C1 9C29 + SygSoq —CygCj9S29 + SygCoq Cyig5Si9 O 


ia) — Soc SioS c 
T 1 = 19°20 19520 i9 QO (eq. 4.5) 
$1gC19Co9 — Cyg529 ~—SygCj9529 — Ci gloq SigSi9 O 
0 0 0 1 


where notations such as S;g = Sin ©,, and Cig = COS ©,, are used. The task now becomes 


one of determining the joint angles ©,,, ©,, and ©,, from this matrix. If one lets 


A, B, Cy D, 
17 -1 sus 
A; B, C; D3 ~*~ 
A, BG, D, 
then, 
sin @,, =+JA> +B; (eq. 4.7) 
and 


sin ©,, =+,/C,+C3 (eq. 4.8). 


Since normally 0 < ©,, < 180 degrees, sin ©,, is chosen to be positive. The three 


angles associated with this solution can now be found as follows: 


©,,= atan2 (sin®,,, C>) (eq. 4.9) 
Q,,= atan2 (B,/sin®,,, -A>/sin®,, ) (eq. 4.10) 
O,,= atan2 (C3/sin©,,, C,/sin®,, ) (eq. 4.11) 


There are two problems which may be encountered with this general type of 


solution. The first concerns the fact that sin©,, may equal zero. In this case, a singularity 


has been encountered and joint;g and jointy,9 axes of rotation are in line with one another. 
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Care must be taken so that the model is designed such that for the starting or reference 
position (Figure 8) this does not occur. However, the singularity may be entered after 


tracking commences. There are two possibilities for handling this. The first is avoidance. 
When sinO,,. nears zero simply set it to some arbitrarily small value. If, however, the 
application demands that the singularity be represented more accurately, the method 
described in [MCMI96b] can be used. In this case, set either ©,, or ©, to its previously 
known value and then solve for the other angle. This can be done by noting that 


cos ©,, = 1. Then 


l= Cin Cots x5 gua c0s (©), — O50) (eq. 4-12) 

Ae 5 eCreGason = Sin(©,|,—©,) (eq. 4-13) 
and 

O13 —OQx = atan > (eq. 4-14). 


Perhaps the best method of handling singularities, a method not used here, is to 
design the model so that singularities are never possible. This can be done for three DOF 
joints by reordering the axes of rotation to ensure that the second axis of rotation can never 
be rotated to align the first and third axes of rotation. For example, if the first and second 
axes of rotation in the model used here (lower body twist and fore-aft bending at the waist) 
are switched in order, then no singularity between the first and third motion axes 1s 
possible. This is due to the fact that humans cannot twist the full 90 degrees required to 


align the first and third axes. 


The second problem concerns the case where ©,, is not limited to an angle between 


QO and 180 degrees. In this case sin®,, can become a negative number. A method for 
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determining when the angle has moved beyond this range of motion is required. If ©,, is 
constrained to being between O and 180 degrees, then the sign of C3 is the same as the sign 
of sin©,, . In such a case, a negative value for C3 indicates that ©,, is no longer between 


0 and 180 degrees. The result of (eq. 4.7) or (eq. 4.8) is therefore chosen to be negative. 


2. Using Position Data 

To demonstrate how position data can be used to determine joint angles, the method 
for determining the clavicle joint angles ©,, and ©,, will be described. Figure 21 will be 
used to show how this can be done. The goal is to determine the position vector between 
the clavicle and shoulder joints. This can be done from position and orientation data given 


by the trackers on the back and upper arm. This vector is then used to determine the 


necessary joint angles. 
The first problem is to determine “Pi6,.13 , the free vector in world coordinates 
describing the relative positions of joint,¢ and joint). Figure 22 shows the vectors needed 


to accomplish this. Once again, one makes the assumption that the trackers are attached to 


links that are rigid bodies, and that the relative motion between tracker and link is nil. With 
this assumption, “p,,,.i.15 » the position of joint;¢ relative to the torso sensor or tracker, and 


ua 


"Pposiths , the position of joint;g relative to the upper arm sensor, are constants which can 


be determined through calibration. If “p,. and “p,,, position vectors are provided by the 
trackers, then 
"Pjointi6 = "Ps + *Rys ” P nositts (eq. 4.15) 


and 


Ls = Ww uas 
Pjointg = "Puas a Soe P posit\8 (eq. 4.16), 
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where “R,, and "R,,, are rotation matrices provided by the trackers. Once ie joinnig and 


"Pjoinig ate found, “Pi6:013 Can be determined from the following relationship: 


Pi6ro1ls = Wererat 7 "Piointt6 (eq. 4.17). 
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Figure 21: Clavicle Position Tracking 
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Equivalently, the position vectors “Pjointig and “p jointig Can also be calculated using 


homogeneous transformation matrices. If ”“H,, and “H,,, are the homogeneous 
transformation matrices provided by the trackers, then calibration can provide the constant 
matrices “Hy osir16 and “A osir1g Which can be used to transform positions reported at the 


trackers to the positions of joint; and joint,g in world coordinates. This is accomplished 


using the following relationships: 


“"H posit16 ="His “Hyosirt6 (eq. 4.18) 
"A positi8 ="Hyas 0 A posit18 (eq. 4.19) 


In this case, the position vectors and rotation matrices previously described can be found 


in the homogeneous transformation matrix with the same sub/superscripts. 

Now that “Pi6:013 has been determined, it is necessary to describe this vector in 
coordinates relative to a coordinate system attached to the upper part of the torso. This is 
necessary since the angles that need to be determined, ©,, and 0,7, are used to position the 
clavicle relative to the torso. The homogeneous transformation matrix H¢ is the position 
and orientation of the upper torso (specifically linkg) in world coordinates. This has been 


previously determined from the tracker on the upper torso. The rotation matrix Rg, the | 


orientation of the upper torso given in Hg, is needed to transform “Pi¢:013 into a vector 


relative to the upper torso. This can be done using the following equation: 


GE 7 (Re)? “Pi6t018 (eq. 4.20). 


The joint angles can now be determined directly from the coordinates given in 


6 : : : 6 
Pi6i018 using trigonometry. If x, y, and z are the coordinates of Pié:01s , then 


©,, = atan2 (z, x) (eq. 4.21) 
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and 


©,, =atan2 (-y, vx? +2’) (eq. 4.22). 


The result of (eq. 4.22) must be normalized to a positive angle for use with the kinematic 
model. 

It should be noted that the position tracking technique shown here is only valid for 
determining up to two joint angles. If the physical limb being tracked can change its pitch, 
azimuth and roll, then there is no way of determining the roll from a position vector 
generated by this technique. This is due to the fact that the position vector generated is 
parallel to the axis of roll. Only hmb azimuth and pitch can be determined. In the case of a 
three DOF limb, it is still possible to determine all three angles. Pieper investigated inverse 
kinematic solutions for six DOF manipulators where three consecutive motion axes 
intersect [PIEP68, PIEP69]. His solution for manipulators where the last three motion axes 


intersect is appropriate for determining ©, , ©, and ©,, given the optimal tracker placement 


shown in Figure 16. A description of this technique can be found in [CRAI89]. 


3. Implementation Specifics 


There are two implementations which were investigated. Each uses the techniques 


shown in the previous two sections to determine joint angles or track positions. 


a. Angles-only Tracking Implementation 

In the angles-only tracking implementation, the objective was to 
demonstrate that four sensors could be used to fully articulate one arm while at the same 
time tracking the torso position and orientation. Though not optimal, this provides a usable 
solution, for the user is able to move around the virtual environment and use one arm. Since 
only tracker orientation data is used, many of the joint angles of the upper body model are 


not articulated. Table 4 shows the joint angles that are computed. The code for calculating 


the joint angles can be found in Appendix A in the calculate_joint_angles( ) function of 
the Body class. In all, 13 DOF are tracked with this solution. 

A major difference between this implementation and the optimal 
implementation concerns the placement of the base tracker. The base tracker is positioned 
on the upper torso as in Figure 17, instead of on the pelvis. Since there is no tracking of the 
lower body, the lower body is not rendered or displayed. The three joints located at the 


upper-waist position, ©,, ©, and ©,, are fixed and allow for the entire back to be treated 


as a rigid body. Since this is the case, the position tracking techniques described in 
section 2 above can be used to determine the base position of the upper body from the 
position and orientation data of the torso sensor. The upper body is translated to this 
position before it isrendered. Angle tracking techniques from section | above are then used 
to determine joint angles given in Table 4, the only difference being that joint angles which 


are fixed (ie. not tracked) result in constant T-matrices. 


, Type 


waist lean angle 
right arm fore-aft angle 
right arm side-side angle 





















right shoulder roll 


right elbow curl angle 
right hand fore-aft angle 


Table 4. Joint Angles Computed - Angles-only Implementation 
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b. Position Tracking Implementation 

In the position tracking implementation, the objective was to demonstrate 
position tracking of the nght clavicle. The code for calculating the joint angles can be found 
in Appendix D in the revised calculate_joint_angles( ) function of the Body class. Table 


5 shows the joint angles that are calculated. The computations used to determine ©,, and 


©,,, are exactly those shown in section 2 above. 


Type 


a 
- —— as 


right a curl torso & position 
upper arm 

right shoulder shrug torso & position 
upper arm 


Table 5. Joint Angles Computed - Position Implementation 





C. SUMMARY 

In this chapter the Polhemus tracking hardware and software were discussed. The 
inverse kinematics computations were also presented. It was hoped that success in tracking 
the clavicle would result in an attempt to use position tracking for determining the joint 
angle for the lower arm. This would result in progress towards the optimal tracker 
placement shown in Figure 16. This was not the case, however, due to limitations in tracker 
hardware. A description of results is contained in Chapter VI. The next chapter, Chapter 


V, discusses methods used to calibrate the system. 
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V. CALIBRATION 


In this chapter a discussion of the methods used to calibrate the sensors and size the 
kinematic model to the user is provided. The goal of this effort is to make the calibration 
process simple to use, yet sufficiently accurate to permit the user to effectively interact with 


his virtual environment. 


A. CALIBRATING SENSORS 

The first step in using Polhemus tracker data to determine joint angles is to 
transform data on the tracker’s position and orientation into data on the tracked limb 
segment’s position and orientation. If the tracked limb segment is treated as a rigid body, 
then it can be assumed that movement of the tracker relative to the limb segment is nil. This 
assumption is only valid if, 1) the limb segment’s frame is attached to the underlying bone 
structure of the limb and, 2) the tracker is attached to the limb in a manner that minimizes 
its motion relative to this same bone structure. This can be done by attaching the tracker at 
a location where there is little muscle or flesh matter between the skin and underlying bone. 
Once the tracker is properly placed, it is the purpose of calibration to determine the constant 
4x 4 homogeneous transformation matrix which describes the relationship between tracker 
and limb segment. This information can then be used to transform tracker data into limb 
segment data. 

Figure 20 and (eq. 4.1) are reproduced from Chapter IV and provided below as 
Figure 22 and (eq. 5.1). They describe the relationships between the world, tracker and 


limb segment frames of reference. 
YH, = “H/H, (eq. 5.1) 
The goal of the calibration process is to determine ‘H,, the homogeneous 


transformation matrix specifying the limb segment frame relative to the tracker’s frame. 
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Figure 22: Relationships Between Frames of Reference 


This can be found by placing the limb segment in a known reference position (ie. "Hy is 
known) and measuring tracker position and orientation, "H,, at that time. Then, (eq. 5.2) 
can be used to compute ‘H,, the desired constant transformation matrix. 


(“H,)! YH, ="H,,”H, ="H, (eq. 5.2) 


1. Angles-only Calibration Technique 
If one intends to only use orientation data for determining joint angles, then the 
calibration process is simplified. In this case silly reference orientation is used, instead of 
orientation and position. The limb segment need only be placed in the specified 
orientation. Additionally, position elements of the homogeneous transformation matrices 
given in (eq. 5.1) and (eq. 5.2) may be ignored, giving the following: 
“R, =”R;R, (eq: 5-2) 


and 
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(*R,)* YR, = ‘R,R, ='R; (eq. 5.4). 

For the angles-only implementation, the reference orientation is that shown in 
Figure 8. In order to position himself for calibration, the user needs to match his body 
position as closely as possible to the reference position. This is done by standing straight 
up and down and placing the nght arm straight down with the thumb forward and elbow 
locked. The user must stand facing in the direction of the world coordinate system x-axis 
with his night shoulder aligned with the world coordinate system positive y-axis as shown 
in Figure 23. Calibration occurs after sensors are initialized on program start-up. The user 
is prompted from the screen to position himself and is given three seconds to do so after the 
“enter” button is pushed. At that time, sensor data is taken and the calibration 
transformation matrices are computed in the calibrate( ) function of the Body class. These 


matrices are then stored for use in their corresponding Body class data members. 


2. Position Calibration Technique 


The position calibration technique is similar to the angles-only technique with the 
exception that initial tracker and limb segment positions relative to each other and the 
world coordinate system becomes important. The calibration position used, however, 1s 
identical to that used in the orientation calibration technique. Figures 23 and 24 show the 
required positioning for calibration of the torso and upper arm sensors used to investigate 
position tracking techniques. As with the orientation calibration technique, the user must 
stand facing in the direction of the world coordinate system x-axis with his nght shoulder 
aligned with the world coordinate system positive y-axis as shown in Figure 23. 
Additionally, the torso sensor is mounted such that it is at the same world coordinate z- 
coordinate as the shoulder and clavicle joints. The upper arm sensor is mounted on the arm 
so that it is at the same world coordinate system x-coordinate as the shoulder and clavicle 


joints. Positioning the body and sensors in this manner allows reported sensor positions to 
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be used in a simple and direct manner for determining joint positions. A more detailed 


explanation follows. 






C) joint 


sensor 





Figure 23: Tracker Position Calibration - Front View 


The purpose of position calibration is to determine the position of a joint relative to 


the sensor which will be tracking it. In the position tracking application investigated here, 


this means determining the constant vectors “p,,,i6 and “"p,,.i:13 (see Figure 21). These 


vectors are then inserted into 4x4 unit matrices to create "H,ir16 and ““H,,o5i:1g Which are 


used in (eq. 4-18) and (eq. 4-19) to convert reported sensor location to joint location. Once 
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Figure 24: Tracker Position Calibration - Side View 


joint locations are known, the inverse kinematic computations described in Chapter IV can 
be accomplished. 


uas 


Measurements to determine °p,,,i6 and ““p,,,;13 are first taken with reference to 


the world coordinate system. The vector from the torso sensor to the clavicle and from the 
upper arm sensor to the shoulder joint are measured with reference to the world coordinate 
system. This is done by using the sensors themselves and by manual means. These vectors 
can be called px. io posits ANA “Pras to positig Since they are measured relative to the world 
coordinate system or the transmitter frame of reference. Figures 23 and 24 show how the 
various Components of these vectors are determined. This 1s done as follows: 


“Dis to positls X-COOrdinate - determined by taking the difference between the reported x- 
coordinate of the upper arm sensor and the reported x-coordinate of the torso sensor. 
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* "Dis 10 positls Y-COOrdinate - this measurement is taken manually by estimating the center 
of rotation of the clavicle joint and measuring the distance to it in the y-direction. 


* "Dis to positis Z-COOrdinate - since the torso sensor is at the same z-coordinate as the 
clavicle joint, this coordinate is set to zero. 


* "Dias 10 positlg X-COOrdinate - since the upper arm sensor is at the same x-coordinate as 
the shoulder joint, this coordinate is set to zero. 


* "Dias to positig Y-Coordinate - taken manually by estimating the center of rotation of the 
shoulder joint and measuring the distance from the spine to it in the y-direction. The 
difference between the reported y-coordinate of the torso sensor and the reported y- 
coordinate of the upper arm sensor is then subtracted from this distance and assigned 
a negative sign. 


* *Duas to positlg Z-COOrdinate - determined by taking the difference between the reported 
z-coordinate of the torso sensor and the reported z-coordinate of the upper arm sensor. 


The manual measurements associated with the y-coordinates of each of these 
vectors were taken and hard-coded into the calibration routine for a single user. This can 
be seen in the code of Appendix D and was done for testing only. The manual 
measurements taken here must correspond with those taken for sizing the model to the user. 
In particular, the measurements and computations which result in the spine-to-shoulder- 
joint length and the spine-to-clavicle-joint length (the link, length) must correspond to 
those input or computed when the kinematic model is sized. Model sizing is discussed in 
the next section of this chapter. 

Once “pis to posits ANd “Pyas to posiig Have been determined, it is necessary to convert 
these vectors into “p,,.in6 and ““p,,,ig respectively. This is done using the following 
equations: 

-1 
eomeae = R,,) iD isticbpasitl6 (eq. 5.2) 
-1 
ia RCS im (CRS, «) ere stnosrlé (eq. 5.6) 


where “R,, and *R,,,, are taken from “H,, and “H,,,, as reported by the torso and upper arm 


sensors at the time of calibration. As previously stated, the constant vectors “p,,.ii¢6 and 


2 


ua 


Pposinig are then inserted into 4x4 unit matrices to create “Hyosi716 and ““Hyosir1g and the 


calibration process is completed. 


B. SIZING THE MODEL 

In order to ensure that the user can effectively interact with his virtual environment, 
the kinematic model is scaled to the user’s dimensions. This results in a virtual human who 
is proportional in dimensions to its user. That way, when the user touches his right shoulder 
with his left finger tips, the virtual human does also. Thus, scaling the user’s dimensions 
into the virtual environment is desired to enhance the perception that the user himself is 
immersed in the virtual environment. It also means that each virtual human can be scaled 
appropriately to objects found in the virtual environment, objects with which the user may 
want to interact. 

The method for sizing the model to the user is straight-forward. Prior to system 
initialization, the user’s measurements in centimeters are taken. These measurements 
include: 1) the spine to shoulder joint length, 2) the upper arm length, 3) the lower arm 
length, and 4) the hand length. Measurements are taken by estimating the center of the 
joints in question. During calibration, the user is prompted for these measurements in the 
calibrate( ) function of the Body class. 

Following input, the code shown in Figure 25 is executed to set the appropriate link 
lengths and joint offsets to size the model. Notice that the input spine to shoulder length is 
used to set the link lengths for links 6, 8, and 17. These correspond to the distances between 
the spine and clavicle joint on each side, and the left and right clavicle link lengths 
respectively. To do this, the clavicle position is arbitrarily assumed to be approximately 
36% of the distance from the spine to shoulder away from the spine. The clavicle lengths 
then become 64% of the manual measurement. The code is written so that setting the link 


length for linkg automatically causes the inboard link lengths for link7 and link, ¢ to be set 


ce 


accordingly. This occurs in the set_link_length( ) member function of the Upperbody 
class which is called by the Body class member function of the same name. The link length 


of link; and joint displacement of joint,¢ are hard-coded to arbitrarily set the size of the 


back, since tracking actually occurs at the torso sensor located between the shoulder blades. 


Setting the joint,;, displacement in the Upperbody class member function 
set_joint_displacement( ) automatically sets the joints displacement and vice-versa, since 


the two are identical. 


// set upperbody dimensions to that of the user 
setmlinicelhengeh (340.21 «0; 

Scum lmm@celvengenl(o, 0.5060 * spine shoulder length) ; 
set_joint_displacement(16, 26.0); 
set_link_length(17, 0.64 * spine _shoulder_length) ; 
set_link_length(8, 0.64 * spine _shoulder_length) ; 
set_link_length(20, uarm_length) ; 
set_link_length(11, uarm_length); 
set_link_length(21, larm_length) ; 
set_link_length(12, laxrm_length) ; 
set_link_length(24, hand_length); 

Set, link  lengen(15 "hand length)% 


Figure 25: Excerpt from Body Class Calibrate Member Function 


Admittedly, the method chosen here for reducing the number of measurements will 
in some cases result in an unacceptable match between the model and user with respect to 
clavicle joint location. A better method would be to submit individual measurements for 
the joint displacement and clavicle length, rather than linking the two to a single input. 
Since the clavicle joint is not a rotary joint which can be specifically located, approximating 
its location is necessary in any event. The method chosen here was found to be acceptable 
for matching the model to the user in a manner that permitted gross motor control of the 


clavicle for testing purposes. 
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OF SUMMARY 

Calibration of the tracking hardware and software model were discussed in this 
chapter. The relationships between the world coordinate system, tracker coordinate 
systems and coordinate systems attached to the skeletal structure were presented. Tracker 
positioning for calibration of angle and position tracking methods was also discussed. The 
chapter concluded with an explanation of how the software model can be scaled to the user. 
The next chapter presents observations concerning the effectiveness of the interface as a 


whole. 
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VI. RESULTS 


The interface described in this thesis was investigated with regards to the three 
major goals for this project discussed in Chapter I. First, the interface needed to be 
effective in driving realistic and reasonably accurate movement of the virtual human. 
Second, the interface needed to be efficient enough to ensure that all actions commanded 
occur in real-time. Third, the interface was to be intuitive and easy to use. The 
investigation was more qualitative than quantitative due to a lack of hardware for 
accurately measuring the user’s movements (determining truth values) for comparison with 
movements the system actually produced. Despite this, there are some observations and 
conclusions worth noting. This chapter provides observations concerning each of the thesis 
goals. The next chapter presents conclusions drawn from these observations and discusses 


future work which can be done. 


A. REPRESENTATION OF HUMAN MOTION 

The system is required to replicate the user’s movements in such a way that the 
motion is realistic and accurate. In this section, realism and accuracy are discussed 
concerning the angles-only implementation and the position tracking implementation. 
Results described here can be found in Appendix E, a demonstration video of the angles- 


only and position implementations of this interface. 


1. Angles-only Implementation 


The realism required should be sufficient to make the user and any other observers 
believe that the virtual human represents an actual human in motion. If measured by this 
standard, then the angles-only implementation can be termed successful. Figure 26 shows 
operation of the angles-only tracking implementation. The system tracks thirteen degrees 


of freedom, including a six degree of freedom torso (position and orientation) and one 
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movement. This is a large and difficult undertaking. It involves double instrumentation and 
tracking Nf the user in a manner that insures one tracking method does not interfere with 
the other. For example, mechanical trackers which are made of metal may interfere with 
the electromagnetic trackers used in the interface. For this reason, the methods for 
determining the accuracy of the system were more subjective and narrow in their scope. 

There were two types of tests used to determine in a limited fashion whether or not 
the system was accurately tracking the user’s movements. Both involved statically 
positioning the user. The first involved observing correlation of joint angles when placed 
in either 90 or 180 degree positions. In general, this test resulted in visually acceptable 
angles being generated. The second test observed the ability to position one body part on 
another, such as placing the finger tips on the shoulder or touching the chest with the palm 
of the hand. The results of these tests, while not perfect, were encouraging. Several 
corrections to the system were made (described below), resulting in somewhat improved 
performance. A more detailed discussion follows. 

The accuracy of the duplicated motion is dependent on several factors. The first is 
the ability of the electromagnetic trackers themselves to accurately report their position and 
orientation throughout the working volume. Polhemus reports that the static accuracy for 
a receiver positioned within 76 cm (30 in) of the transmitter is 0.15 degrees RMS for 
receiver orientation [POLH93]. Operation beyond this range or in the presence of 
ferromagnetic interference will reduce accuracy. Since the transmitter’s antennas are 
mounted to the ceiling and the user moves freely about it, actual operation is often beyond 
this range. Also, the environment in which the device was tested had numerous objects 
which could cause ferromagnetic interference, including metal light fixtures, a metallic 
drop floor and the computer and monitor used to run the interface. If a hypothetical case is 
considered where all four sensors are within 76 cm of the transmitter’s antennas and the 


user’s spine to finger tip length is 90 cm, and if the arm and hand are extended straight out 


fie, 


to the side and all four sensors are off by 0.15 degrees in the same direction, then the hand’s 
orientation would still only be off by 0.15 degrees with an accumulated position error of 
approximately 0.24 cm at the finger tips. However, given the working environment and 
test results, it is likely that the errors are greater. 

Livingston and State investigated the registration of electromagnetic trackers at the 
University of North Carolina [LIVI96]. Though they used Ascension Technologies Flock 
of Birds trackers, their work illuminates the inaccuracies inherent in today’s 
electromagnetic trackers and shows the difficulties which must be overcome to improve 
registration of these trackers. Their orientation error data for a single Flock of Birds sensor 
(less sensitive to ferromagnetic interference than Polhemus) was taken in a spherical 
working volume of two meter radius in a similar environment. Using 11,419 
measurements, the data shows an average error of 3.34 degrees with a standard deviation 
of 1.31 degrees and recorded minimum and maximum errors of 0.13 and 20.46 degrees 
respectively. If we apply the average orientation error in their findings to the example 
above, then the accumulated position error of the finger tips now becomes approximately 
5.28 cm. This error is noticeable in some applications. It may explain in part why 
positioning the hand over or touching other body parts sometimes resulted in a perceptible 
disconnect between reality and the virtual representation. On the other hand, it can be 
difficult to observe only three degrees of error at any given joint angle. This may explain 
why errors were not as noticeable using the 90 degrees joint angle test. 

Another cause of inaccuracies is the motion of a tracker relative to the underlying 
skeletal structure which itis tracking. During testing, it became evident that this was in fact 
the case for the torso sensor. Motion of either clavicle would cause the harness on which 
the sensor was mounted to move, moving the sensor relative to the spine. This was due in 
large part to the motion of the shoulder itself and the shoulder blades around and on which 


the straps of the harness rest. Unfortunately it is difficult to keep the clavicle immobile 
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while moving the arm. This resulted in a noticeable tilt in the body position from the 
vertical ee the arm was raised. The rest of the tracked limb segments (arm and hand) 
retain the proper orientation relative to world coordinates due to the manner in which joint 
angles are calculated. If the torso sensor is held in position on the spine by another 
individual, then the represented body orientation and posture is much closer to reality. To 
improve torso sensor tracking a new harness was designed. The new harness utilized a 
single strap suspender design with shoulder straps splitting high up the back and running 
as close as possible to the neck. The straps were then crossed in front and attached to a belt 
wrapped around the torso just below the waist. This helped avoid much of, but not all, the 
movement of the sensor when the clavicles were used. Similar problems were not observed 
with respect to the remaining limb segment trackers. 

Infidelity in the model chosen can also cause accuracy problems. Since the clavicle 
was modeled but not tracked, the effect is as though it was not modeled at all. As 
previously mentioned, it is difficult to move the arm without moving the clavicle associated 
with it. For movements such as scratching one’s back by reaching up and over the shoulder, 
large amounts of clavicle angles are induced (depending on the individual user). This 
results in a 1a of correlation between reality and the representation. In effect, the 
reachable workspace of the user extends beyond what is reachable for the model. For 
scratching the back, the result is that the icon’s hand hangs above where the user’s hand is 
actually positioned. This problem can only be corrected by tracking the clavicle motion 
and applying it to the model. 

A second problem with the model was observed regarding the imposition of joint 
limits. The joint limits shown in Table 3 were arrived at by observing apparent limitations 
in movement of the human joints in the direction of the modeled joints. In fact, only a few 
restricted movements were observed in isolation from other movements. The range of 


movements available to the user, a result of combining motion around all three axis of 
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rotation, require these model limits be expanded. This was discovered when the icon’s 
posture ee from various positions to others during testing. Removing most of the 
joint limits (as has been done in Appendix A) resulted in elimination of the problem. Since 
the physical joint limits of the user himself will prevent unrealistic articulation of the figure, 
it would seem there is no need for joint limits in the model. 

The method chosen to handle singularities in the system, avoidance, does not result 
in noticeable inaccuracies. One might expect that skipping over singularities might result 
in a discontinuity in motion. Attempts to place the system in a singularity or move the 
system through a singularity show no difference from other types of motion. 

Inaccuracies in the method chosen to calibrate the system can cause problems. 
During testing of the elbow joint, it was noticed that the elbow lagged the actual elbow joint 
angle by approximately 10 degrees. It was discovered that this was a result of assuming 0 
degrees joint angle at the time of calibration, when in fact this is not possible for most users 
without hyperextension of the elbow. This resulted in a computed elbow joint angle which 
was always less than the actual joint angle by an amount equal to the difference between 
actual and reference positions at the time of calibration. To correct this, the reference 
position was changed to a position where the elbow is bent 90 degrees with the arm forward 
and thumb pointed up. This placed the lower arm and hand parallel to the world coordinate 
system x-axis and required that the two reference calibration matrices associated with the 
lower arm and hand be modified in the code. The problem was no longer observed. It 
should be noted, however, that accuracy is still effected by how closely the user can 
position himself to the reference position. 

Accuracy of motion replication involves timing also. It is not enough that static 
positions be replicated accurately. The goal is for the motion to be replicated with the same 
rates and accelerations. If the system exhibits minimal lag and can position the virtual 


human in real-time, then the rates and accelerations displayed will be close to that of the 
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actual user. An attempt at quantifying system lag was not made for lack of suitable 
measuring equipment. Subjectively, there is a perceptible lag in the virtual human’s 
movements, but it is not large for even the fastest of arm movements. Polhemus provides 
the latency of its tracker as four ms from the center of the receiver measurement period to 
the beginning of transfer from output port [POLH93]. Itis likely that most of the perceived 
lag, however, is a result of overhead due to transfer of data from the output port to the SGI 
and device driver buffers, and subsequent rendering. When compared with the interface 
developed by McMillan [MCMI96b], lag is much improved. This is probably due to the 
fact that McMillan’s interface used a more complex model (Jack) and worked in a 
networked virtual environment (NPSNET). Excessive lag in this case, resulted in a loss of 
fluidity in the rendered motion. This highlights the need for identifying system choke 
points in order to ensure maximum efficiency when working in the networked 


environment. 


2. Position Implementation 


From the outset, it became apparent that it would be difficult to use the Polhemus 
electromagnetic trackers for position tracking. It was possible to observe control of the 
virtual human’s clavicle in all four directions, but only if the user’s position remained very 
close to the calibration position. The representation exhibits constant jitter due to sensor 
drift. Movement away from the calibration position results in a lack of controllability due 
to poor tracking system registration. Polhemus reports a static position accuracy of 0.08 
cm RMS for x, y or z receiver position when within 76 cm of the transmitter’s antennas 
[POLH93]. In a simple test, two sensors were attached 30 cm apart to a plastic ruler. The 
magnitude of the position vector between them was computed and output to the screen. The 
sensors where moved throughout a one meter cubed workspace corresponding to the 


area in which the clavicle was tested. Readings changed rapidly and varied between 
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20 cm and 54 cm. _ Livingston and State’s data for raw position error, taken 
camera with their measurements for orientation error, show an average error of 
5.69 cm with a standard deviation of 4.55 cm and minimum and maximum errors of 
.11 and 32.17 cm respectively [LIVI96]. Two sensor positions are required to track 
the clavicle. This being the case, it is clear that these errors are too large to allow 


accurate position tracking of a nominal 14 cm long clavicle. 


B. EASE OF USE 

The interface is easy to set-up and use. The user must have four measurements 
taken, including spine to shoulder length, upper and lower arm lengths, and the hand length. 
After donning the equipment and turning on the Fastrak system, the program can be run. 
The user inputs his measurements and then stands in the calibration position. Pressing the 
“enter” key causes a three second delay, after which calibration measurements are taken 
and tracking commences. Following this, it only takes a few seconds to display the 
replicated image. The entire process takes approximately five minutes. 

Control of the figure 1s as easy as controlling one’s own body. The intuitive nature 
of the interface is born out by the fact that small children can successfully use it when told 
to position the figure in a variety of postures. Sensor mounting does not restrict movement, 
if care is taken to make sure the upper arm sensor is not mounted too close to the elbow. 
Sensor wiring is perhaps the most encumbering aspect of the interface. Routing sensor 
wiring up the arm, through the arm and torso sensor straps, and down the back helps to 
avoid entanglement, though the user must be mindful of wiring that trails back to the 
Fastrak control box. Sensor wiring and transmitter range restrict use to approximately 10 
-12 feet from the transmitter’s antenna. Within this workspace, however, the user is 


relatively free to move about. 
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VII. SUMMARY AND CONCLUSIONS 


A. SUMMARY 


This thesis addressed the problem that virtual environments (VE’s) do not possess 
a practical, intuitive, and comfortable interface that allows a user to control a virtual 
human’s movements in real-time. The approach was to develop an interface for the upper 
body, since it is through this part of users’ anatomy that they interact most with their 
environment. Implementation included construction of a 24 DOF Danevit-Hartenberg 
kinematic model of the upper body. The model is manipulated in real-time using 
orientation data from electromagnetic motion tracking sensors placed on the user. 

Electromagnetic trackers were chosen because of their 6 DOF tracking capability, 
availability, and low cost. Their small size makes them easy to attach to the user. 
Calibration of these sensors is a Straight-forward process. The user simply positions 
himself in a reference position for a single set of sensor readings. The device takes 
approximately one sixth the time to don and calibrate as do mechanical interfaces with 
similar capability. 

Research resulted in an interface that is easy to use and allows its user to interact 
with a VE. The device tracks thirteen degrees of freedom. Upper body position is tracked, 
allowing the users to move through the VE. When using the device, the user has the feeling 
of being immersed in the VE. The interface can be used for a variety of applications which 


do not require higher levels of precision. 


B. CONCLUSIONS 

The electromagnetic tracking systems available today lack sufficient accuracy and 
registration to enable their use as true six DOF trackers. It was hoped that by using a 
traditional kinematic notation together with six degree of freedom sensors, a smaller 
number of sensors and less encumbering system would result. While this may be true, in 


many applications it would be desirable to mount a six DOF tracker on each limb, rather 
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than reduce the number of trackers. This enables one to use the redundant tracking data to 
improve accuracy for precision applications, such as sighting and firing a rifle in the virtual 
environment. Accurate and well registered six DOF electromagnetic trackers could be used 
in many applications in which the current trackers cannot be used. This would be of great 
benefit to the designers and users of virtual environments. 

One method of improving the registration of electromagnetic trackers is to calibrate 
the device within a specified workspace and provide a look-up table for error correction 
during operation. This is exactly the work being undertaken by Livingston and State at the 
University of North Carolina [LIVI96]. Their experimentation with a Faro Metrecom 
IND-1 mechanical tracker and Flock of Birds sensor has resulted in a method which 
reduced the average position error by 78% and the average orientation error by 40%. They 
found, however, that orientation errors depend not only on the tracker’s position, but also 
on its orientation. Their lack of success in reducing orientation error is attributed to their 
original assumption to the contrary. A 6D look-up table is therefore required for 
orientation. This makes a look-up table calibration method impractical for orientation. 
However, if the look-up table method is applied to position data, it should reduce errors 
enough to significantly improve registration. This could make electromagnetic trackers 
usable as a true six DOF trackers for some applications, including this one. 

The usability of electromagnetic trackers for military applications is greatly 
reduced by several aspects of their design. First, their sourced nature requires the user to 
remain within range of the transmitter. In some combat training scenarios this would be a 
severe restriction. Second, their susceptibility to ferromagnetic interference can greatly 
impede their use in the military environment. Care must be taken to avoid operation near 
ferromagnetic objects, such as nfles, certain types of combat gear, electronic equipment, 
and the metal bulkheads of ships, tanks and other vehicles or simulators. Finally, the 


system used here was tethered. This means that large or cluttered working volumes can be 
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difficult to work in. Military applications require large working volumes, greater freedom 
of — and freedom from ferromagnetic interference. This makes sourceless and 
untethered systems that are robust in an environment high in electromagnetic interference 
very desirable. 

The design of this interface, as with any other engineered product, reflects trade- 
offs between achievable capabilities. For example, by making the system easy to calibrate 
some precision is sacrificed. Also, by reducing the complexity of the model for increased 
speed and efficiency, some accuracy in replicating the user’s movements or simulating his 
reaction to the environment is lost. For example, a dynamic model and force feedback 
could be used to create a more acceptable training environment, but at a cost of much 
greater complexity and system overhead. The question is whether an acceptable balance 
can be achieved between trade-offs when considering the overall purpose of the system. 
One must recognize, however, that limitations of available technology may preclude 
successful achievement of all design goals. 

The success of this interface is application dependent. Consider that success is 
related to the ability of the user to use the interface to accomplish tasks and learn in the 
virtual environment. The angles-only implementation of the system is usable for tasks 
which can be accomplished with only gross motor control. These include various forms of 
locomotion and dance, use of arm signals, communicative interaction with virtual devices, 
moving virtual objects, and playing games that require lower amounts of coordination, such 
as virtual handball with a large or slower moving ball. Itis unlikely that this interface could 
be used for applications requiring higher levels of precision, such as aiming and firing a 
rifle. It remains to be seen whether the interface can be used in the networked virtual 


environment and for what purposes. 
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C. FUTURE WORK 

The angles-only interface can be improved in several basic ways. Additional 
sensors could be added for articulation of the left arm, and the system could be modified to 
allow articulation of the head. The ability to provide the user a view from the eyes of the 
virtual human is also desirable. Finally, the rendered appearance of the model can be 
improved by incorporating one of several graphic models currently available in academia 
and the commercial sector. 

Additional testing of the interface is necessary to determine its strengths, 
weaknesses, and suitability for the networked virtual environment. Though a major 
undertaking, a quantitative evaluation of the accuracy with which the figure can be 
positioned is desirable. It is also important to determine the lag in the interface. Results of 
these tests could be used to identify possible improvements. More subjective testing of the 
ability of users to accomplish various tasks should also be conducted. Following these 
quantitative and qualitative tests, the interface could be implemented in a networked virtual 
environment and the tests repeat. 

The issue of using electromagnetic tracker position data should be revisited. A 
look-up table calibration method such as the one described in [LIVI96] and a suitable filter 
for reducing the jitter associated with sensor drift could be implemented. The interface 
could then be modified for increased accuracy or for use with fewer sensors. Such a change 
could result in significantly improved application of the interface. 

Finally, research in other areas of modeling and tracking may result in more 
effective interfaces. For example, by modeling the body using a quaternion notation one 
can eliminate singularities [COOK92]. Rate tracking through positions that otherwise 
would have been singularities can occur. Suitable quaternion filters can be designed to 


combine tracking inputs and enhance the output of the interface [BACH96b]. 
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Alternate new tracking technologies should also be investigated. Spread spectrum 
electromagnetic tracking and artificial vestibular (inertial) systems are particularly 
promising. The spread spectrum system would enhance accuracy, resolution, response, 
robustness, and sociability over current electromagnetic systems. An artificial vestibular 
system is the only sourceless system, allowing for excellent robustness and sociability. 
Perhaps a combination of tracking technologies will result in the best system, with 
advantages of one technology compensating for disadvantages of another. 

This thesis provides a starting point for those interested in developing better 
interfaces for users of Virtual Environments. It can be found on the Internet at http://www- 
npsnet.cs.nps.navy.mil/npsnet/publications.html. Code used in this thesis can be accessed 
via anonymous file transfer protocol (FIP) at ftp://ftp-npsnet.cs.nps.navy.mil/pub/ — 


skopowski/ArticulatedHuman.tar.Z. 
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APPENDIX A: ANGLE TRACKING SOFTWARE 


link.h 1 


Ti KKK KKK KKK KEKE KEKE KKK KEKE KKK KR KEKE KERR KKK KER KEKKEKKEKKEKEKKEKREKRKEKKEKEEKE 


// FILENAME: link.h 

// PURPOSE: declarations for the link class 

if 

// AUTHOR: P F Skopowski 

i ia Dee:s 26 Nov 35 

// COMMENTS: definition and some functions of the link class 


faye KEK KKK RK KK KKK KKK KEKE KEK KKK KKK KEKE KR KER KKRKE KER KEKRKEKEKEKE KE KKK KKEEEK 


#ifndef LINK_H 
#define LINK_H 


#inelude <GL/gl.h> 
#include <GL/glu.h> 
#include <GL/glx.h> 


class Link 
{ 
DUDi ae + 
Link (double, double, double, double, double, double, 
fleat. = 050, Lioat = 0.07- Eleat=.0. 05 floaters ey. 


// Function: rotate (double angle) 
// Purpose: set the link's joint angle 


void rotate (double angle) 
{ 
if (angle < min_joint_angle) { 
angle = min_joint_angle; 
} 
if (angle > max_joint_angle) { 
angle = max_joint_angle; 
} 
jJoint_angle = angle; 


// Function: rotate_increment (double increment_angle) 
// Purpose: increment the link's joint angle 


void rotate_increment (double increment_angle) 


{ 
double angle = increment_angle + joint_angle; 
rotate (angle); 


oh 


link.h 2 


// Function: draw() 
// Purpose: draw the link in the proper position/orientation 


void draw() 

{ 
glTranslated ((GLdouble) inboard_link_length, 0.0, 0.0); 
glRotated ((GLdouble) inboard_twist_angle, 1.0, 0.0, 0.0); 
glTranslated (0.0, 0.0, (GLdouble) joint_displacement) ; 
qlRotated ((GLdouble) joint_angle, 0.0, 0.0, 1.0); 
ff eraw lenge = O20) { 

G@awDianona(0.0, 0.0, 0.0, draw_length, draw_width, 
draw_depth, draw_offset) ; 


// Function: reset() 
// Purpose: reset the link's joint angle to its zero posit 


void reset() 
{ 
jJoint_angle = initial_joint_angle; 


// Function: set_joint_displacement (float displacement) 
// Purpose: set the link's joint displacement 
// Returns: TRUE 


6 SSS 55 S55 35 5555555555 55655555 5555555555 55 S55 555 Se 
int set_joint_displacement (float displacement) 
{ 
joint_displacement = displacement; 
return 1; 
} 
a ee 


J//SeUnemr on. Set inboard link length(ftloat length) 
// Purpose: set the link's inboard link length 
// Returns: TRUE 


int set_inboard_link_length(float length) 
{ 

impoana. link length = length; 

PeeUrh. 1; 
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link.b 3 


// Function: set_draw_length(float length) 
// Purpose: set the draw_length of the link 
// Returns: TRUE 


eee 
int set_draw_length(float length) 
{ 
draw_length = length; 
Febiene i- 
} 
[Ym a a a nn = = 


// Function: set_draw_width(float width) 
// Purpose: set the draw_width of the link 
//eReturns: TRUE 


| Kietintntnteietnetetentatentntetentetntetetetateteteteteteatetetetateteteaatetetntateateatatataatatetetatateta 
int set_draw_width(float width) 
{ 
Graw_width = width; 
TeC Usain ; 
} 
[ [mmm rr rr rr eee 


// PuUneceELOnN: Getodhkaw Grrseer() 
// Purpose: get the draw_offset of the link 
// Returns: draw_offset 


float get_draw_offset () 


{ 
return draw_offset; 


} 
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link.h 


protected: 


er 
#endif 


double inboard_link_length; 
double inboard_twist_angle; 
double joint_displacement; 
double initial_joint_angle; 
double joint_angle; 

double min_joint_angle; 
double max_joint_angle; 

int number_outboard_links; 
float draw_length; 

float draw_width; 

float draw_depth; 

float draw_offset; 


ole iey  hunet.ons 
void drawDiamond(float x, float y, float 2, 


float length, float width, float depth, float offset); 


void computeNormal (const float* a, const float* b, const float* c, 
fleat *result); 
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link.cc 1 


// KEK KKK KKK KEK KEKE KEK KKK KK KEK KEKE KKK KKK KKK KEKE KKK KKK KKK KKK KKK KKK KEK 


// FILENAME: link.cc 

// PURPOSE: function definitions for the link class 
le 

// AUTHOR: P F Skopowski 

J{/ OATS: 265Nov 95 

// COMMENTS: Link constructor and drawDiamond function 
// MODIFIED: 9 Mar 96 


// KEKE KKK KEKE KKK KEK KEKE KEK KEKE KKK KEKE KKK KEKE KKK KEK KKK KKK KKK KKK KK KK KEKE 


tine lade *link.h” 
#include <math.h> 


FS SS SS SSS 
// Fum@eion: Link (deuble ill, double itave@deuble ja; 

7 : Gouble ija, double min_ja, double max_ja, 

1s Pr ploatlal, float dw, floattdd = Elear dort ) 


// Purpose: constructor of the link type 
// Returns: link class object 


Link: bink (double ill, double ita, doubYesjdpeeaouble i )4, 
double min_ja, double max_ja, 
fieat dl, floatbedw/etloakddyerloact dott) 


inboard_link_length pela 
inboard_twist_angle = ita; 
joint_displacement = jd; 
inital joint_ang@le = ija; 
JOintlaneme =) 14; 
Min_jJOImMel_ angle = min_ja; 
max_joint_angle = max_ja; 
draw_length = dl; 
draw_width = dw; 
Grawadepth = dd; 
Graw_offset = doff; 
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link.cc 2 


| 
// Function: drawDiamond(float x, float y, float z, 

// : float length,float width, float depth, 

if “erloat Oneset) 

// Purpose: draw a diamond with end at (x,y,z2) 

// : use specified parameters 

// : Graws diamond along the x-axis 

a : center of diamond can be offset 
ee 


V6lCmeen:serawblamona(tlioat x, float y, float z, 
float length, 
tPlGacs wldch, 
float depth, 
float offset) 


J/ Gloat. x era: end of the diamond in 3-space. 


float midpoint; // x coordinate for waist vertices of the diamond 
float halfwidth; // half the width 
float halfdepth; // half the depth 


Piacem meo) [3] 5 // array to hold coords for the diamond vertices. 
ELOae~ nil 3); // array to hold the normal vector 

// Compute the x-axis midpoint. 

Migpoint=length*offset; 

// Compute half the dimensions. 


ha pewegine= wiGdteh/ 2.0; 
Haltdepen = depth/Z.0; 


Jp ECG EICes . 


ROO | =<; 
Deptt | =v; 
p[0] (2) =z; 


ele) Ol=xemidpoint; 
Deiat l=y ; 
p({1] {2)=z+halfdepth; 


BZ tO l=xemcwoin et. 
p(2] (1)=yt+thalfwidth; 
p[2] (2]=2; 


PlaltOl=x+midpoint; 


Silo Jay 
Dl) (2Z)=z2-haltdepenh, 
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link.cc 3 


p({4] [0]=x+midpoint; 
p[4][1]=y-halfwidth; 


p[4] (2]=2; 
pi5) (Ol=x+length; 
p(5] (1lJ]=y; 
Blslt2i=2; 


PEeOueslonrer(i.C7 1.0, 1720 )s /h@Set the color to Wiree: 


// Compute and set normal for the first side 
computeNormal(p[0], pili], p[2], n); 
glNormal3fv(n); 


glBegin(GL_POLYGON) 
givervexsivipl0] 

] 

] 


); 
GQ Veree cs evi [1] ) > 
glVertex3fv(p[2]) 


gl Bwel(); 


td 


V7guectlorsr (020, 0.0, 0.0); /f Set the color tov black. 


// Compute and set normal for the first side 
computeNormal(p[0], p[2], p[3], n); 
glNormal3fv(n) ; 


glBegin(GL_POLYGON) 
glVertex3fv(p[0] 

] 

] 


e 


e 
me 
) . 


i 


glVertex3fv(p[2 
glVertex3fv(p[3 
glEnd{); 


J foleolorst (i. 0, 110) .1<.0)2 // See theveolor to wnilee- 


// Compute and set normal for the first side 
computeNormal(p[0], p[3], pl4], n); 
gliNomnal3iv(n)-; 


glBegin(GL_POLYGON) ; 
glVentex3i(p(0])- 
glVertex3fiv(p[3]); 
glvertex3fv(p[4]) 
glEnd(); 


? 


/ (/Giselors tee 0.5, 05 5).; /7 Set the color to gray 
// Compute and set normal for the first side 


computeNormal(p[0], pl4], pli], n); 
glNormal3fv(n) ; 


Mi 


link.cc 4 


glBegin(GL_POLYGON) ; 
glVertex3fv(p[0]); 
glVertex3fv(p[4]); 
glVertex3fv(p[(1)); 
glEnd(); 


VrogleGolers&(0.5;, 0:5, 0.5); // Set the color to gray. 


// Compute and set normal for the first side 
conputelermal (pik, DIZ], pli], n); 
giNemmdl3iv in) ; 


glVertex3fv(p[5]); 

glVertex3fv(p[2]); 

Gil vertexstv(p[1)) 
glEnd(); 


glBegin(GL_POLYGON) 


f 


PpeuGovorsc( 1.0, 1.0, 1.0); // Set the color to white. 


// Compute and set normal for the first side 
GoupuecNopmal(pl>)]; pi3], Ppl2], n); 
glNormal3fv(n)j; 


glBegin(GL_POLYGON) ; 
glVertex3fv(p[5]); 
eivertexsiv(p[3)); 
glVertex3fv(p[2]}) 
Gena te): 


e 


, 


Promeevomiti. 0, 90.0, 0.0); // Set the color to black. 


// Compute and set normal for the first side 
computeNormal(p(5], p[({4], pl[3J, n); 
glNormal3fv(n) ; 


glBegin(GL_POLYGON 
glVertex3fvi(p[5 
glVertex3fvi(p[4 
glVertex3fv(p[3 
GLEna() ; 


); 
); 
) 


* 
° 


) 
] 
} 
Ie 
PPOUuGetoror reo, b.0, 1.0); V7 cee the color to white. 
// Compute and set normal for the first side 


COompuEeNGrmal(pl5), pli], pl4], n); 
gGlNomal3tv(n) ; 
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link.cc 


giBegin(GL_POLYGON) ; 
Givercexsivie (5 ||); 
glVertex3fv(p[1]); 
gliVertex3fv(p[4]) 
glEnd(); 


f 


// the diamond is drawn. 


// Function: computeNormal (const float* a, 
V7 * const float* c, float *result) 


i i ted 


const tloak* so. 


// Purpose: compute normal vector to a triangular polygon 


void Link::computeNormal (const float* a, 
float *result) 


fle aee ees |; 
float Ys]; 
float magnitude; 


// compute the first vector 


Dee =— tC a0), 
oe ete tol ee dale; 
Mew 2 |i al 2s 


// compute the second vector 


Viol = -c:10)] -=salel, 
pee ea = al Bi 
alee eee a 


/7 GOmpute the Cross DrOGucE. Vector 


Resmi (Oo) = xl vi2i= xXi2zl vit 
recmre (i)! = x[(2ie* y[(O) = x(0le 7 i2ic 
Result [2] = x(0] * vil] =.x(1) * viol: 


//normalize the result 

magnitude = sqrt(result[(0] * result[0] 
result[(2] * result[2]); 

result[({0] = result[0]/magnitude; 

result[{1] = result[1]/magnitude; 

result(2] = result[(2]/magnitude; 


oP) 


ee ee te ee te 


Const £loat= bh 


+ result[1] 


eonst, floac* ter 


* result(1]) 


link1.h 1 


aes KEK KKK KKK KEKE KKK KKK KEKE KEK RK KKK KKK KKH K KKK KKK KKK KEKKEKKEKEKEKKE 


// FILENAME: linkl.h 

// PURPOSE: declarations for the linkl class 
He 

// AUTHOR: P F Skopowski 

// DATE: 26 Nov 95 

// COMMENTS: definition of the linkl class 


age KERR KEKE KKK KEK KKK KKK KKK KKK KK KEKE KKKEKKEKKEKKE KKK KKKKEKKKKEKEKEK 


#ifndef LINK1_H 
#define LINK1_H 


fameiiae “link. h" 


Glasswotikie: public Link 
{ 
public: 
Pirie bak. (0.0, 0.0,  0.50,-0.0, 
eoemermeg0 00" "O20, 0.0, 0:0; 0.0) 
{ } 


private: 


ee 


#endif 
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link2.h 1 


lies KHEKEKKEKKKKEKKKKEKKEKEAKKEK KEK KEKKKEKKAEKKEKEKEKAEKKEKEKEKKEKKKKKKKKKEKKEKKE KEK 


// FILENAME: 1link2.h 

7// PURPOSE: declarations for the Immmk2 class 
i] 

// AUTHOR: P F Skopowski 

// DATE: 26 Nov 95 

// COMMENTS: definition of the link2 class 


if KEEKEKEKKKKEKEKKKEKKKKEKKKKEKKEKKEEKEKEKEKKEKEKEKEKEKKKEKKEKKEKKEKKEKKEKEKKKEEK 


#ifndef LINK2_H 
#define LINK2_H 


fingilude “link. h”® 


elacswitink2 : public Link 


{ 
Pablic: 
Binks () solneak <6 (O10, 90-0 00> IOC e277 One 
300.0; Ser.0> 020° Org 0.0) 
{ } 
private: 
7 
#endif 
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link3.h 1 


// kKaeweKe keke keke KKK KKK KKK KKK KRKKRK KKK aK KKK KKK KKK KKK KKK KKK KKK KKK KK 


// FILENAME: ines 2 

// PURPOSE: declarations for the link3 class 
i4 

// AUTHOR: P F Skopowski 

// DATE: 26 Nov 95 

// COMMENTS: definition of the link3 class 


// kKaekKkK KK KKK KKK KKK KKK KKK KK KKK KKK KK KK KKK KK KKK KKK KKK KK KK KKK KK 


#ifndef LINK3_H 
#define LINK3_H 


tinegwude “link.h” 


Slacssc eGink3s = public Link 
{ 
Public: 
igus: bank (020, 90.0, 0.0, O20; 
on we oO ely ao 1 O.5, 6.5, 0.5) 
os 


private: 


ye 


#endif 
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link4.h 


Hays KEKKKKKEKKKK KK KKK KEKKEKE KKK KEK KEKE KKK KK KKK KKK KKEK EK KEK 


// FILENAME: link4.h 

// PURPOSE: declarations for the link4 class 

ie 7 

// AUTHOR: P F Skopowski 

// DATE: 26 Nov 95 

// COMMENTS: definition of the link4 class 

es KEKE KEKKEKKEKKKKEKKKKEEKKEKKEKEKKEKEKEKEKKKKKEKEKKKKKEKKEKEKKKKEKKEKKEKEKKEKE KEK 


#ifndef LINK4_H 
#define LINK4_H 
fincluvee “link.h” 


class Link4 public Link 


{ 
puUbisie- 
Link4 () Link i7.5,. 90.0, -0 40 sects e- 
OSC,, Ve; O07 ese .C 
i) 
private: 
ee 
#endif 
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O-@) 


link5.h 1 


-7 KR KKK KEK KEK KEK KEK KKK KEKE KKK KKK KEK KEK KK KKK KKK KK KEKE KEK KKEKKKKKEKE KK KK 


// FILENAME: link5.h 

// PURPOSE: declarations for the linkS5 class 
ca 

// AUTHOR: P F Skopowski 

// DATE: 26 Nov 95 | 

// COMMENTS: definition of the linkS5 class 


od, KK KEK KKK KKK KK KK KKK KREMER KKEKKKKEKREKKEKEKEKKEKKEKKEREKKEKEK KES 


#ifndef LINKS_H 
#define LINKS_H 


fineluae “link.h?’ 


Clacs ibincs: = public Link 


{ 
Duele : 
bimesa) = Link { -0.0, 9020, OrOr 90.0, 
OeGisna Swe 0; Ce Crews. Oo. OO) 
{ } 
private: 
ses 
#endif 
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link6.b 1 


as KEKKKKKEK KEK KEKE KEKE KKK KEKE KEKE KKK KE KEKE KEKE KEKE KEKE KKK KEK KEK KKK KKK KEE 


// FILENAME: link6é.h 

// PURPOSE: declarations for the link6é class 
fi. 

// AUTHOR: P F Skopowski 

// DERE: 26 Noy 95 

// COMMENTS: definition of the linké class 


i / KKK KE KKKEKKE KEKE KK KEKE KEKE KK KEKE KEK EEK KEE KKK KEKE KKK KEKE KK KK KKK KEKE KK 


#ifndef LINK6_H 
#define LINK6_H 


fimelude “link. h" 


Slacs GInk6e =“public Link 
{ 
publ Le: 
ume: dermk (“0207. 90.0,. 0.05. 1802.0, 
359.0); 225.20 ,. 2650s oy Op eoe a Ons) 


oe 


7// Funetion: draw() 
// Purpose: draw the link in the proper position/orientation 


void draw({) 


{ 

// araw the torso 

glTranslated ({(GLdouble) inboard_link_length, 0.0, 0.0); 

glRotated ((GLdouble) inboard_twist_angle, 1.0, 0.0, 0.0); 

glTranslated (0.0, 0.0, (GLdouble) joint_displacement) ; 

gikotated ((Ghdouble) (jointeangle —990. 0). 7070 meer) eo) 

drawDiamond(0.0, 0.0, 0.0, draw length, draw lwiden, 
draw_depth, draw_offset) ; 

// draw the head 

glRotated ({(EGLdcuble) 45:07, 1.0, 0.07. 0707. 

drawbDiamond (draw length; 0.0; G.0,- 10.075 1020 lOO ae oe 

glRotated ((GLdouble) =45.0, 1.0, 0.0, 0707; 

// araw the nose 

glPushMatrix(); 

gifranslated((draw-length + 5.0), 0.0, 0.0); 

GuROcaeea (90.0, 0.07 1.0, 0:0); 

Gpanbiamonc Os0, O50, 0.0, 6:5, 320, 3,07 22); 

glPopMatrix(); 

Glunecatea ((Gudouble) 90.0, 0.0, 0.0, 1:0); 

} 
private: 


13 


#endif 
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link7.h 1 


// 
// 
es 
ke 
// 
c7 
i? 
// 


KKK KKK KKK KKK KKK KEK KEKE KKK KEK KK KEK KKK KKK KKK KK KK KEKE KKK KK KEKE K 


FILENAME: Linky on 
PURPOSE: declarations for the link7 class 


AUTHOR: P F Skopowski 
DATE: 26 Nov 95 
COMMENTS: definition of the link? class 


KKK KKK KEK KEKE KEK KEK KKK KKK KKK KEK KK KEK KKK KK KEKE KKK KKK KKKKE KEK EK 


#ifndef LINK7_H 
#define LINK7_H 


time rede “link.h” 


elaiss Wink? > public Link 


{ 
joelol liter 
Meni ty). bank { 7.5, 90-0; 22.5, Oey, 
Oper Ooeo. O60) O20, 0.0, 0.0) 
a) 
private: 
}; 
#endif 
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link$.h 1 


IY 
a. 
ee 
ay 
Ay 
Lg 
ta 
be 


KEKE KKK KKK KK KK KEKE KRK RK KK KE KKK KKK KK KER KERR KKK KKK KEKE KEKE KK 


FILENAME: lines 
PURPOSE: declarations for the link8 class 


AUTHOR: P F Skopowski 
DATE: 26 Nov 95 
COMMENTS: definition of the link8 class 


KKK KKRREKRKKE KEKE KEKE KEKE KK KEKE KKK KEKE KEKE KK KEK RK KKRKEKKE KK EK KEKRKEKEERK 


#ifndef LINK8_H 
#define LINK8 H 


fimclwude “link.h” 


eracce Linke : public Link 


{ 


3 


Sule: 
linea’ () 2 ieak. «(£030 90.0 8 20. 0; Geigy 
=20%0, 5060, 12.5, 6.5, 6.5, @em5) 
tae 


private: 


#endif 
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link9.h l 


fe i KKK KKEKKEK KK KKK KKK KEK KEKE KKK RK KKK KE KKK KEK KKKEKKEKEKEKKEKKKEKK 


// FILENAME: link9.h 

// PURPOSE: declarations for the link9 class 
// 

// AUTHOR: P F Skopowski 

// DATE: 26 Nov 95 

// COMMENTS: definition of the link9 class 


lied KKK KEKE KEK KKK KKK KKK KEKE KE KKK KKK KEK KKK KKK KKK K KKK KKK KKEK 


#ifndef LINK9_H 
#define LINK9_H 


fimeliae “link.h” 


elass Link9 : public Link 


{ 
public: 
moe iink (17.5. 9020, 0.0,.990.0, 
So sO eGo. OO, 0.0, 0.0, 020) 
{ } 
private: 
tei 
#endif 
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link10.h 1 


// 
ee 
// 
if 
Ge 
taf, 
el 
// 


KERR KKK KKK KK RR KK RE KKK ERK KEKE KR KK KEK RK KKK KKK EKER KKK KK KKEEE 


FILENAME: Ababa <1 0}esial 
PURPOSE: declarations for the link10 class 


AUTHOR : P F Skopowski 
DATE: 26 Nov 95 
COMMENTS: definition of the link10 class 


KEKE KEKE KKK KREKRKKEKKEKKKEEKRKEKEKEKRKEKEEKREKRKEKEKRKEKKKKRE KEE 


#ifndef LINK10_H 
#define LINK10_H 


Famevude “link h*” 


elass*iinkl0 : public Ligk 


{ 
public : 
Binkl0() « Link (O20 290-0 020A 02 oF 
—310. 0, 360.0," 00> O70 O00) 
a, 
private: 
re 
#endif 
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link11.h 1 


// KERR KEKE KEKE KKK KK EEK KKK KE KK KKK KKK KKK KKK KKK KKK KK KEK 


// FILENAME: imei. h 

// PURPOSE: declarations for the linkl1 class 
Lf 

// AUTHOR: P F Skopowski 

// DATE: 26 Nov 95 

// COMMENTS: definition of the linkil class 


| es KEK KEKE KKK KKK KEKE KEKE KK EK KEKE KEKE KEKE KKK KEKE KKK KKK KEKE KE KEEEK 


#ifndef LINK11_H 
#define LINK11_H 


fimewuce: ?link.h” 


etess Einkll : public Link 


{ 
pubic : 
Mec) cobinKo (30-0,-90.0, 0.0, O30), 
SoG Oe soO,O. 22.5, 1020, 10.0, 0.2} 
{ } 
private: 
ra 
#endif 
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link12.h 1 


al 
foh 
// 
ry 
hes 
ae 
// 
if 


KERR KKK KK KKK KK KKK KKK KKK KKK KKK KKK KKK KEKE KKK KKK KK KKK KKK KK 


FILENAME: linki2.h 
PURPOSE: declarations for the link12 class 


AUTHOR: P F Skopowski 
DATE: 26 Nov 95 
COMMENTS: definition of the link12 class 


KK KK KKK KKK KKK KEKE KKK KKK KEKE KKK KEKE KK KEK KEKE KK KKK KEKKEK KK KEKE 


#ifndef LINK12_H ; 
#define LINK12_H 


Tonciuae “link.h” 


Glacstbinkl2 >< pulsiic Lank 


{ 


PP 


jeu olisiere 
Game? ()° + Link “(222-572 9040. Ome i Se0- 
0.0, 170.0) “2535, 9°05 270,02) 
it 


private: 


#endif 
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link13.h 1 


// KE KK KK KEK KEK KEKE KEK KEK KEK KKK KE KEKE KKK KEK KEKE KKK KKK KEKE KKK KKK KK KKK KKK 


// FILENAME: link13.h 

// PURPOSE: declarations for the linki3 class 
ff 

// AUTHOR: P F Skopowski 

// DATE: 26 Nov 95 

// COMMENTS: definition of the link13 class 


jag KEE KEKE KK KEKE KEKE KEKE KK KEK KKK KEKE KKK KEKE KKK KKK KKK KKK KKK KEK KEK KE KKK KK KEK 


#ifndef LINK13_H 
#define LINK13_H 


fHimelude “link.h” 


Clacs=ainkl? : public Link 
{ 


Pulte’: 
Menlo oeink (e25.0, 000, 0.0; 0.0, 
=Jam0, 90.0, 0.0, 0.090.0, 0.0) 
{ } 
private: 
}; 
#endif 
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link14.h 1 


lay KERR KE KKK KEKE KK RE K EK KEKE KKK KEKE KK KKK KEK KKK KEKE KKK KKK KK KKK KKK 


// FILENAME: linki14.h 

// PURPOSE: declarations for the link14 class 
va 

// AUTHOR: P F Skopowski 

// DATE: 26 Nov 95 

// COMMENTS: definition of the linki4 class 


Va KREKKKKKREKE KEKE KKEKEKRK KERR KEE KEKE KERR KKK KKEKKEKKEKRREK 


#ifndef LINK14_H 
#define LINK14_H 


#include *“link.h” 


elec inked : publie Link 


{ 
je UN ost atte 
Lemaieea() = Linke 02055 -=90.0,. 0.07 -90.0, 
ThoO.O, “OO 10), 0... On GeeeOeC aro) 
{3} 


private: 


tae 


#endif 
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link 15.h 1 


// 
GE 
// 
// 
Ue 
// 
// 
// 


KEKE KKKAK EKER EKER KEKE KEKE KKK KKK KEKE KEK KEK KKK KEKKKKEKEEE 


FILENAME : linkrS.nh 
PURPOSE: declarations for the link6 class 


AUTHOR: P F Skopowski 
DATE: 26 Nov 95 
COMMENTS: definition of the link15 class 


KKK KKK KEKE KEKE KKE KEK KEKKEKEK EK KEKEKKKEKEKKEKEKKE KKK KKK KKK 


#ifndef LINK15_H 
#define LINK15_H 


fHimeiuae “Link.h*” 


elass Ginkl5 +: public Link 


{ 


jr 


Pubic: 
Pini omomiglmk (0.07 8=9020". 0.0, 0:0, 
SO 0" 90.0; lisp Oy S 
{ } 


void draw({) 

{ 
// draw the hand 
glTranslated ((GLdouble) inboard_link_length, 0.0, 0.0); 
glRotated ((GLdouble) inboard_twist_angle, 1.0, 0.0, 0.0); 
giTranslated (0.0, 0.0, (GLdouble) joint _displacemenc) ; 
Qiveeaeoa | (Ghcdouble) joint_angle, 0.0, 0.0, 1.0); 
mingeacea) {-90.0, 0.0, 1.0, 0.0); 
Glreocacea { 9020, 1.0, 0.0, 0.0); 
Grawbiamona(0.0, 0.0, 0.0, draw_length, draw_width, 

Graw_depth, draw_offset) ; 


Jj eosaw the thumb 

Gipegtacea {((Gldeouble) =-30.0, 0.0, 1.0, 0.0); 

Cmawenamonc oslo. 0, 0.0, {(.75 * draw _length), 3.0, 3.0, .5); 
Gir eceacea ((Gldcuble) 30.0, 0.0, 1.0, 0.0); 

Giretacea ((Glhdeuble) 90.0, 0.0, 0.0, 1); 

} a 


private: 


#endif 
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link 16.h 1 


iA KEKE KKK KK KEK KEK KEKE KEKE KKK KE KEK KEK KEK KEKE KEKE KK KEK KKK KKK KEK KKK KEKE 


// FILENAME: link16.h 

// PURPOSE: declarations for the link16 class 
es 

// AUTHOR: P F Skopowski 

i 7, DATE: 26 Noy 495 

// COMMENTS: definition of the link16 class 


7} eK eK RE KKK EKER KK RAKE EK KKK KKK KEK KEKE KK KEK KES RK KX 


#ifndef LINK16_H 
#define LINK16_H 


timeclude ” lamk .h” 


elaccelinmels : public Link 
{ 
PubmiG: 
bamkne() 22 ehank (=725.. 9040.) 22.5). 1 S0e0r 
T402057° 210.205 “eG aoe 020) 
{ } 


private: 


#endif 
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link21.b 1 


Ta KKEKKK KKK KEK KEK KEK KKK KEK KKK KEK KEK KEKE KKK KKK KKK KEKE KKK KKK KK KKK 


// FILENAME: link21.h 

// PURPOSE: declarations for the link21 class 
ay 

// AUTHOR: P F Skopowski 

J//™RATS: 26 Nov 95 

// COMMENTS: definition of the link21 class 


i KEK K KKK KEK KK EK KKK KEKE KKK KKK KKK KKK KKK KK KKK KK KKK KKK KKK KK 


#ifndef LINK21_H 
#define LINK21_H 


fincluwg@e ”link.h” 


Clacceuimk2! : public Link 


{ 
Public: 
Pek 2 LInk 9 f222).5,2 90.0; . 0.0, Cay 
poo 07 60.70, 25.5, ° 9.0;. 9:0; 0-2) 
{ ) 
private: 
}; 
#endif 
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link23.h 1 


as 
Va 
fay 
ft 
fd 
oe 
he 
// 


KEKE KEKK KE KK KKEK EK EK EK KEK KEK KEK EK RK KKK RE KEKE KE KK KEKE KEK EK EK 


FILENAME: lames nh 
PURPOSE: declarations for the 1ink23 class 


AUTHOR: P F Skopowski 
DATE: 26 Nov 95 
COMMENTS: definition of the 1ink23 class 


RREEKEKEKKKE KK EKKKREKKEKEKEKKK KEKE KE KEK EKER KKEKKKEKK KKK KEK KKK KKK KEKE EK 


#ifndef LINK23_H 
#define LINK23_H 


minclude *link.h’” 


Slace fink2s = spublic Link 


{ 
PupUle: 
binkZ3s es bink { ©.0; 90.0,. O.0}) 90207 
=180.0,. 020% “GF07 (05-00. 0 oe) 
ae 
private: 
ie 
#endif 
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// 
ah 
// 
a 
Pe. 
p) 
es 
es 


KKEEKKK KE KKEKKKEKKEKEKRKEKKEK KEKE KKK KRKEKEK EKER KKK KKKKEKKKKEKEKKKEEEK 


FILENAME: link24.h 
PURPOSE: declarations for the link24 class 


AUTHOR: P F Skopowski 
DATE: 26 Nov 95 
COMMENTS: definition of the link24 class 


KKKKKEKKKKKKEKRKKKEKKEKEKKEK KKK KKK KE KKK KE KKKKEKKKKEKRKKKKEKKKEKEEK 


#ifndef LINK24_H 
#define LINK24_H 


fenclude “link.h’” 


elass Gink24 : public Link 


{ 


es 


Dube: 
mimi sot) eo eink 020 ,.-90.0, 0.0, 0.0, 
SlOvUpwoOnore 17.0, 3.0, 8.0, 0.2) 
{ } 


void draw() 

{ 
// draw the hand 
glTranslated ((GLdouble) inboard_link_length, 0.0, 0.0); 
glRotated ((GLdouble) inboard_twist_angle, 1.0, 0.0, 0.0); 
glTranslated (0.0, 0.0, (GLdouble) joint_displacement) ; 
Menouwacea ((Ghoacuble) yointlangle, 0.0, 0.0, 1.0); 
Gieoearea (-90.0, 020, 1.0, 0.0); 
Gimocavrea, (=-90.0, 1.0, 0.0, 0.0); 
drawDiamond(0.0, 0.0, 0.0, draw_length, draw_width, 

draw_depth, draw_offset) ; 


// draw the thumb 
Gineeaceds ((Ghcouble) —30.0, 0.0, 1.0, 0.0); 
Saawebamone wed, 0-0, 0.0, (.75 * draw_length), 3.0, 3.0, .5}; 
Gineearea ((GhLdouble) 30.0, 0.0, 1.0, 0.0); 
Glnpe@taced ((GhLdeuble) 90.0, 0.0, 0.0, 1); 
} = 


private: 


#endif 
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ae KKK KKK KEK KK KEK KK KEKE KKK KKK RK KK KKK KKK KKK KKK KEK KKK KKK KKK KKK KK KK 


// FILENAME: upperbody.h 

// PURPOSE: declarations for the upper_body class 
77 

// AUTHOR: P F Skopowski 

// DATE: 26> Nov 95 

// COMMENTS: definition of the upperbody class 


ies KKREKKKEKKKEKKKKKKKEKEKKEKEK KEK KEK KK KK KKEKKKKEKEKEKKKEKKKKEKKEKKKKKKEEEK 


#ifndef UPPERBODY_H 
#define UPPERBODY_H 


#inclu@e *~linkl.h” 
Fincluge “Tink2 .h* 
pimeluce ” links .h” 
#include “link4.h” 
pimelude “links sh“ 
Sineclude *~link6.h” 
fincilude “link? .h* 
Pine lude. “linké.h” 
#include ”link9.h” 


#include ”"link10.h” 
Himelude “linkl).h” 
finclude “link1l2.h’ 
fimelude *“linkl3.h” 
finelude “linkwd4.h7 
#include “ lwsak15.h* 
foneciude “linkils. h” 
fine bude “link2Z1.h” 
#include ”“link24.h’” 


class Upperbody 
{ 
private: 

Panik? diet: 
Eink2 dank? 
Panmk3 links; 
tonk4 link4; 
banks lanks: 
binke linkG- 
binky lank? ; 
Link8 links; 
Link9 link9g; 
LankLo link 10: 
Sas oN IMMA aly py call, 
ee ee elem? 
Japa eals al ksGay <a tie, 
Dinka slim 4- 
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Pomel S. LlinkisS, 
foe 6 linkl6; 
Binks linkl7; 

fines Linkl3; 

memeiO inkl9; 
Bimkil Jink20; 
imei link21i; 
Pintle Iank22; 
Pomie4 Link2 3; 
Pinks Link24 ; 


public: 
Upperbody () ; 


void rotate (double *); 

void rotate_increment (double *); 
wera draw () ; 

void reset(); 

Mie Ssetelink Jength(int, float) ; 


Ine, Set.7O1nt-displacement(int, float); 
on 


#endif 
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alk KKK KEKE K KKK KKK RK KK KER KKK KKK KKK RE KEKE KKK RK KEKE 


// FILENAME: upperbody.cc 

// PURPOSE: functions for the upperbody class 
1g 

// AUTHOR: P F Skopowski 

J// DATE: 26 Nov 95 

// COMMENTS: functions for the upperbody class 


T 7 KKK KKK KEKE KEK KEK RK KEKE KK KEKE KEK KKK KEKEKEKK KKK KEKE KEKE KEKE KKE KEK 


#include “upperbody.h” // inelude the header file for the class 


// Function: Upperbody () 
// Purpose: constructor of the Upperbody type 
// Returns: Upperbody class object 


Upperbody : :Upperbody () // constuctor 
{ } 


// Function: rotate (double angle[25]) 
// Purpose: set joint angles to the angles specified 


vold Upperbody::rotate (double angle[(25]) 
{ 
lintieretate tangle[l])); // set each link’s joint to the new angle 
link2= rotate (angle([2]); 
links .mne@tate(angle[3]); 
link4.rotate(angle[4]); 
links .vrotate(angle(s]); 
link6.rotate(angle[6]); 
link? .rotete(angle(7]); 
link8.rotate(angle[8]); 
link9.rotate(angle[9]); 
linklO. rebate (angle (10) ); 
link1l1.rotate(angle[11]); 
link12.re@ate (angle([12]); 
link13.rotate(angle[13]); 
link14.rotate(angle[14] 
link15.rotate(angle[15] 
1link1i6.rotate(angle[16] 
( 
( 
( 
( 
( 


f 
f 


u 


f 


link17.rotate(angle([17] 


? 


) 

) 

) 

) 

link18.rotate(angle[18] ) 
1ink19.rotate(angle[19]); 

) 

) 

) 

) 

) 


? 


link20.rotate (angle[20] 
link21.rotate(angle[21] 
lMimk22 rotate rangle | 22) 
link23.rotate(angle[23] 
link24.rotate(angle[24] 


f 


Ul 


° 


? 


, 
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upperbody.cc 2 
ee a wm a ne = Se 
// Function: rotate_increment (double increment_angle[25] ) 
// Purpose: increment joint angles by the angles specified 
I ee 


void Upperbody::rotate_increment 
{ 


(double increment_angle[25]) 


lishied<a ie 


rotate_increment(increment_angle[1]); // increment each joint angle 


link2.rotate_increment (increment_angle[2]); 
link3.rotate_increment (increment_angle[3]); 
link4.rotate_increment (increment_angle[4)); 
linkS.rotate_increment (increment_angle[5]); 
link6é.rotate_increment (increment_angle[6]); 
link7 .rotate_increment(increment_angle[7]); 
link8.rotate_increment (increment_angle[8]); 
link9.rotate_increment (increment_angle[9]); 


HG ay iar 
Pai mied 1%, 
Letmie) 2 
dL big) <li ee 
le 4 
Jamies < 
Im Ge 
Drie 7s 
00) <a sh 
link19. 
Aero ele 
Taink2a 
Te 2 


linke3 


fei mice 4. 


BOLCaLe Inenmement (lncrement_angle[10] ); 
rotate_increment (increment_angle[11]); 
rotate_increment (increment_angle[12]); 
rotate_increment (increment_angle[13]); 
rotate_increment (increment_angle[14]); 
rotate_increment (increment_angle[15]); 
rotate_increment (increment_angle[16]); 
rotate_increment (increment_angle[17]); 
rotate_increment (increment_angle[18]); 
rotate_increment (increment_angle[19]); 
rotate_increment (increment_angle[20]); 
rotate_increment (increment_angle[21]); 
rotate_increment (increment_angle[22]); 
.rotate_increment (increment_angle[23]); 
rotate_increment (increment_angle[24]); 


wee i a a a es ee 


Jy eoumet lon: draw () 

// Purpose: draw the upperbody 
void Upperbody: :draw() 

{ 


.draw 
.draw 
.draw 
.draw 


seat ak 
bet < 
iim 3 
lank 
linkS .draw 
link6é.draw 
glPushMatr 
link7.draw 
link8.draw 
link9.draw 
1link10.draw ( 
link11.draw ( 


; // draw each link, starting at the waist 


f 


f 


f 


(); // after drawing upper torso, remember where it was drawn 
// start drawing left side from the shoulder 


f 


() 
() 
() 
() 
() 
() 
ax 
() 
(); 
Oye; 
1 
) . 


f 
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Link12. draw 
linklig2e@raw 
link14.draw 
link15.draw 
glPopMatrix 
link16.draw 
link 72draw 


( 

( 

( 

( 

Gre // come back to the upper torso 

( 

( 
1ink18. draw ( 

( 

( 

( 

( 

( 

( 


; // start drawing the right side from the shoulder 


link19.draw 
link20 .draw 
link21.draw 
link22- draw 
link23.draw 
link24.draw 


V7 sSunetion=: “reset () 
// Purpose: reset all joint angles to their zero position 


void Upperbody: : reset () 

{ 
linkl.reset 
link2.reset 
link3.reset 
link4.reset 
linkS.reset 
link6é.reset 


Oe // reset the joint angle in each link 
( 
( 
( 
( 
( 
link7.reset ( 
( 
( 
C 
Ec 


ee 
) i 
); 
)3 
); 
yo 
Vs 
link8.reset(); 
link9.reset (); 
link10.reset(); 
link1l1.reset(); 
link12.reset(); 
link13.reset ( 
link14.reset ( 
link15.reset ( 
link16.reset ( 
link17.reset ( 
linki8. reset ( 
link19. reset ( 
link20.reset ( 
link21.reset ( 
link22.reset ( 
link23.reset ( 
link24.reset ( 


) 

di 

i 

di 
i 
i 
i 
yi 
Mi 
Ys 
i 
a 
i 
ye 
i 


f 
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// Function: set_link_length(int link, float length) 
// Purpose: set the link length of a specified link 
// Returns: TRUE if successful 


Piewupperboay -:set_link=lengemine Jink,@float’ length) 
{ 
iiveciccess = 0- 


Gwieen (link) { 


case 1: 
success = linkl.set_draw_length(length) ; 
ite success) { 
success = link2.set_inboard_link_length(length) ; 
} 
break; 
case 2: 
success = link2.set_draw_length(length) ; 
if (success) { 
success = link3.set_inboard_link_length(length) ; 
} 
break; 
case 3: 
success = link3.set_draw_length(length) ; 
if (success) { 
Suceess )—) link4. Set. inboard link _lenogth(length) ; 
} 
break; 
case 4: 
success = link4.set_draw_length(length) ; 
if (success) { 
success = linkS.set_inboard_link_length(length) ; 
} 
break; 
case 5: 
success = link5.set_draw_length(length) ; 
if (success) { 
Siieeces>s =) linke-.set_inboard_Jlink_length(length); 
} 
break; 
case 6: 
success = link6.set_draw_width(2*length) ; 
if (success) { 
success = link7.set_inboard_link_length (length) ; 
success = linki6é.set_inboard_link_length(-length) ; 
} 
break; 


124 


upperbody.cc 5 


case 7: 
success = link7.set_draw_length(length) ; 
if (success) { 
success = link8.set_inboard_link_length(length) ; 
} 
break; 
case 8: 
success = link8.set_draw_length(length) ; 
if (swecess) { 
success = link9.set_inboard_link_length (length) ; 
} 
break; 
case 9: 
success = link9.set_draw_length(length) ; 
if (success) { 
success = linki10.set_inboard_link_length(length) ; 
} 
break; 
Casean0: 
success = link10.set_draw_length(length) ; 
if (success) { 
success = JianWel: seteinboard_linkslength (length) ; 
} 
break; 
case ll: 
success = linkil.set_draw_length(length) ; 
TE eeCsuccess) { ; 
success = link12.set_inboard_link_length(length) ; 
} 
break; 
Gace 12: 
success = linki12.set_draw_length (length) ; 
if (success) { 
success = linki3.set_inboard_link_length(length) ; 
} 
break; 
case 13: 
success = link13.set_draw_length(length) ; 
it (success) { 
success = linki4.set_inboard_link_length (length) ; 
} = 
break; 
case 14: 
success = link14.set_draw_length (lengthy: 
if (success) { 
success = link15.set_inboard_link_length(length) ; 
} 
break; 
case 15: 
success = linki5.set_draw_length (length) ; 
break; 
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case 16: 
success = link16.set_draw_length(length) ; 
if (success) { 
success = linkl17.set_inboard_link_length(length) ; 
} 
break; 
case 17: 
success = link17.set_draw_length(length) ; 
if (success) { 
success = 1]1ink18.set_inboard_link_length(length) ; 
} 
break; 
case 18: 
success = ]ink18.set_draw_length(length) ; 
1f£ (success) ( 
success = 1ink19.set_inboard_link_length(length) ; 
} 
break; 
Gace, 19. 
success = ]ink19.set_draw_length(length) ; 
if (success) { 
success = link20.set_inboard_link_length(length) ; 
} 
break; 
case 20: 
success = link20.set_draw_length(length) ; 
if (success) { 
success = link21.set_inboard_link_length(length) ; 
} 
break; 
case 21: 
success = link21.set_draw_length(length) ; 
it (success) { 
Siecess = link22.set_inboard_link_length( length) ; 
} 
break; 
case 22: 
success = link22.set_draw_length(length) ; 
i~ (success) { 
success = link23.set_inboard_link_length(length) ; 
} 2 
break; 
case 23: 
success = link23.set_draw_length(length) ; 
if (success) { 
success = 1]1ink24.set_inboard_link_length(length) ; 
} 
break; 
case 24: 
success = link24.set_draw_length(length) ; 
break; 
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default: 
break; 


} 
return success; 


// Function: set_joint_displacement (int link, float displacement) 
// Purpose: set the joint displacement of a specified link 
[7 eherumnc- 9 TrRURsIf Sugcess ful 


int Upperbody::set_joint_displacement (int link, float displacement) 
{ 


ie success = 0; 
switch (link) { 


case 1: 
success = linkl.set_joint_displacement (displacement) ; 
break; 

case 2: 
success = link2.set_joint_displacement (displacement) ; 
break; 

case 3: 
success = link3.set_joint_displacement (displacement) ; 
break; 

case 4: 
success = link4.set_joint_displacement (displacement) ; 
break; 

case 5: 
success = linkS5.set_joint_displacement (displacement) ; 
break; 

Case 6; 
success = link6.set_joint_displacement (displacement) ; 


break; 
case 7: 
success = link7.set_joint_displacement (displacement) ; 
if (success) { 
success = linkl6.set_joint_displacement (displacement) ; 
success = link6é.set_draw_length (displacement/ 
link6é.get_draw_offset()); 
} 
break; 
case 8: 
success = link8&.set_joint_displacement (displacement) ; 
break; 
case 9: 
success = link9.set_joint_displacement (displacement) ; 
break; 
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case 10: 
success 
break; 

case ll: 
success 
break; 

case 12: 
success 
break; 

case 13: 
success 
break; 

case 14: 
success 
break; 

case 15: 
success 
break; 

case 16: 


link10 


Ri 8 eile 


Lomi 


i g Heelies 


Meee 4 


Links 


.set_joint_displacement (displacement) ; 


.set_joint_displacement (displacement) ; 


.set_joint_displacement (displacement) ; 


.set_joint_displacement (displacement) ; 


.set_joint_displacement (displacement) ; 


.set_joint_displacement (displacement) ; 


success = link16.set_joint_displacement (displacement) ; 
if (success) ( 
success = 
success = 


link7.set_joint_displacement (displacement) ; 
link6é.set_draw_length(displacement/0.8) ; 


} 
break; 
case 17: 
success 
break; 
case 18: 
success 
break; 
case 19: 
success 
break; 
case 20: 
success 
break; 
CaserZl: 
success 
break: 
case 22: 
success 
break; 
case 23: 
success 
break; 
case 24: 
success 
break; 
default: 
break; 


TIO gig ley gaa 


Pinks 


link19 


link20 


Mink? 1 


link22 


eum? 2 


inks 4 


set_joint_displacement (displacement) ; 


.set_joint_displacement (displacement) ; 


.set_joint_displacement (displacement) ; 


.set_joint_displacement (displacement) ; 


.set_joint_displacement (displacement) ; 


.set_joint_displacement (displacement) ; 


.set_joint_displacement (displacement) ; 


.set_joint_displacement (displacement) ; 


} 
return success; 
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as KEK KKKKKEKEKKKKEKKKE KE KKK KKK EK KKEK KEKE KKK KEK EKRKEKKEEKEKKSE 


// FILENAME: lowerbody.h 

// PURPOSE: declarations for the Lowerbody class 
v/ 

// AUTHOR: P F Skopowski 

//{/ DATE: 26 Nov 95 

// COMMENTS: definition of the Lowerbody class 


Hae KKKEKEKKKEKEK KEK KEKE KKE KEKE EK KEK KEKE KRKKEKE KEE KKK KEKE KKK KEKKEKKEKKEKEK EK 


#ifndef LOWERBODY_H 
#define LOWERBODY_H 


#include <GL/gl.h> 
#include <GL/glu.h> 
#include <GL/glx.h> 
#include <math.h> 


class Lowerbody 
{ 
private: 
Vold GrawbDiamond(flodt, Eloat, fleait, tloau, Flcac, 
float, float); 
void computeNormal (const float* a, const float* b, east Saioat* c, 
float *result); 


jebletlaker 
Lowerbody () {}; 


// Function: draw() 
// Purpose: draw the lowerbody 
ey : the lowerbody is static and does not move 


vold draw() { 


glinGteted(-90.0, 0.0, 0.057 lo); 
arawhramonad(d.0, 0.0, .0 <0, 1256 )- 132 0, 6 ace 
GIRGeaced(30.0, 05077050 lea 
Grawbiamond(10.5, 0.6, 0.0, 25.55 9.5, S25, Cae 
Geawmiamona s6.0, 0.0, 0.0, 27.55 8-55 26.5,.00 een 


GuRetateat-o0.0, 0.0, 
drawDiamond(10.5, 0.0 
drawDiamond(36.0, 0.0 
CUke@taced(120.0, 0.0,50.0, 1.0)-7 


bo 
~] 
Wn 
oO 
Ww 
oo 
Wl 
=) 
bh) 


IB 


#endif 
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// KEKE KEKE KKK KKK KEKE KKK KEK KEK KEK KEK KEK KKK KEKE KEK KEK KEKE KKK KKK KEK EK K KKK 


// FILENAME: lowerbody.cc 

// PURPOSE: function definition for the lowerbody class 
ee 

// AUTHOR: P F Skopowski 

// DATE: 26 Nov 95 

// COMMENTS: drawDiamond function 


Tf KKK KKK KKK KEK KEK KEKE KEKE KKK KEK KEK KKK KEK KEKE KEKE KEKE KK KKK KK KKK KEK KEK 


#include “lowerbody.h’” 


ee i i ne ee wn = == 
// Functron. GdrawDiamond({(float x, float y, float z, 

ie Mmenteat lengtn,cloat width, float depth, 

ge : float offset) 

// Purpose: draw a diamond with end at (x,y,Z) 

17 : use specified parameters 

ge : Graws diamond along the x-axis 

i] : center of diamond can be offset 

[fmm nr rr rr rr eee 


void Lowerbody::drawDiamond(float x, float y, float z, 
float length, 
float width, 
float depth, 
float offset) 


Pe ELOates YZ; end of the diamond in 3-space. 


EPiGaeemicpoint; // * coordinate for waist vertices of the diamond 
float halfwidth; // half the width 
float halfdepth; // half the depth 


Ploae workcarlsS |: /(eearay Eee nold coords for the diamond vertices. 
PloCaten ts): V7 array to hold the normal vector 
// Compute the x-axis midpoint. 
Miapeime=length*oLriset: 

// Compute half the dimensions. 


halfwidth = width/2.0; 
halidepeh = depth/ 2.0; 


JJ -Vereiees. 


p[0)] [0] =x; 
DO aly 
PIO =z; 
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p[L)(Ol=xtmrdpoint- 
p[1] [(1)}=y; 
p(1] (2)=z+halfdepth; 


p[2] [O0]=x+midpoint; 
p(2] (1])=y+halfwidth; 
p[2](2]= 


p[3] (0) =x+midsoime; 
ibe =: 
Blas h(2 f=z-healligepth; 


mie O j=x+midpoint; 
p[4 ee halfwidth; 
p[4] (2]= 


mics to l=x+lengeh; 
Pistilli l=yv; 
Elo i2)=2; 


mgucolorshias0, 1 Onl. 0); // Set the color to white. 


// Compute and set normal for the first side 
computeNormal(p[0], pl1], p[2], n); 
giNormalstiv(n) ; 


glBegin(GL_POLYGON) ; 
Givertexse7 (2 10l); 
qivert ema (o (11) 
glVertex3fv(p[2]) 
glbnd(); 


; 


mealeoloret (0.0, O.C7,.0.0) // Set the color to black. 


// Compute and set normal for the first side 
ComeuteNormeal(p[0], p[2], pl[3], n); 
giNormal3s iv (m) ; 


glBegin(GL_POLYGON) ; 
OG Vercexs iy (ei) ); 
}); 

}) 


f 


glVertex3fv(p[2 
glVertex3fv(p[3 
gle ( ):; 


jo cleovensmimeO, 1.0, 1.0); /i Sete the color Eo white, 
// Compute and set normal for the first side 


ConpuLenormal(pl0], pi[3], pl4)], ny); 
glNormal3fv(n); 
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glBegin(GL_POLYGON) ; 
glVertex3fv(p[0]); 
glVertex3fv(p[3]); 
glVertex3fv(p[4]) 
glend () ; 


, 


Veemleolor3st (0.5, 0.5, 0.5); 7 aeceetne. color to gray . 


// Compute and set normal for the first side 
computeNormal(p[0], p[4], pli], n); 
glNormal3fv(n) ; 


glBegin(GL_POLYGON) ; 
glVertex3fv(p[0]); 
glVertex3fv(p[4]); 
glVertex3fv(p[1]) 
glEnd(); 


é 


PyOleelowew 0 «5,2 0/5;.20.5) ; // Set the color to gray. 


// Compute and set normal for the first side 
SonpurencCrmama (Si; plz), pli], n); 
Gliveormalsiv (my, 


glBegin(GL_POLYGON 
glVertex3fv(p[5 
glVertex3fv(p[2 
glVertex3fv(p[1l 
glEnd(); 


is 
a: 
1 


SS ee at 


Peleclousi().0, 1.0, 1.0); Vyasa the color to white. 


// Compute and set normal for the first side 
computeNormal(p([5], pl3], p(l2], n); 
Gitllermalsityv(n) ; 


glBegin(GL_POLYGON) ; 
givertex3ivip([5]); : 
glVertex3fv(p[3]}); 
glVertex3fv(p[2]) 
glEnd(); 


f 


Prue orer st (Ose. Oe. 0) 7 // Set the color to black. 
// Compute and set normal for the first side 


Gemoucentorumal (p15), pl4), pis], n); 
glNormal3fv(n) ; 
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glBegin(GL_POLYGON) ; 
glVertex3fv(p[5]); 
glvertex3fv(p[4]); 
Givertexsav io lei: 
glfndait); 


TPPLCOUOrS Et Opel 0), 1k.) r/ Set the color to white 


// Compute and set normal for the first side 
computeNormal(p[5], pl1], pl4], n); 
glNormal3fv(n) ; 


glVertex3fv(p[5]); 

giVertexs iy (otl)); 

glVertex3fv(p[4]) 
glEnd(); 


glBegin(GL_POLYGON) 


° 


f 


// the diamond is drawn. 


a a ee eg SS SS 
(7 -FUNCELOnN: -computeNormaliconst ~Eloat* a, const: Eloar.o, 
ii. 7 const  float* cy rlicat *nesule) 

// Purpose: compute normal vector to a triangular polygon 
OE 


void Lowerbody: :computeNormal (const float* a, const float* b, const float* c, 
flea * result) 


tloaeese(s1: 
floace vis |; 
float magnitude; 


// compute the first vector 
(al b[O] - a[0); 


ceo ieee); 
X12 Die at2); 


// compute the second vector 


y(0] = c[0] - a[0]; 
i) = (Oe lauls 
y(2] = c[2] - ala); 
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// compute the cross product vector 
moet oie= ol) * yi2z) - x[2)>* y[1); 
Pest | = x[2) * y[0]) = x[0) * y[2); 
Saerrei2) = x(0)-* yl) - x1) * yO); 


//normalize the result 


magnitude = sqrt(result[0] * result[(O] + result[1]) * result[1] + 
Besuie( 2) *=resule(2]); 


result[0] = result[(0) /magnitude; 
result[1] = result[1]/magnitude; 
result[2]) = result [2] /magnitude; 
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// 
// 
if 
ty 
// 
is 
// 
£1 


KEKE K KK KKK KKK KEK KEK KKK KKK KKK KK KEK KEKE KE KKK KEK KKK KKK KKK KEK KKK KKK 


FILENAME: body.h 

PURPOSE: declarations for the Body class 
uses angles-only tracking technique 

AUTHOR: P F Skopowski 

DATE: 1 Mar 96 

COMMENTS: definition of the Body class 


KER KKK KK KK KEK KKK KKK KK KK KEKE KEK KEKE KEKE KKK KKEKKKK KK KKK K KKK KKK KK KEK 


#ifndef BODY_H 
#define BODY_H 


#define PF_CPLUSPLUS_API 0 
#include <Performer/pf.h> 
#include “upperbody.h” 
#include ”“lowerbody.h” 
#include ”“FastrakClass.h” 


class Body 


{ 


private: 
Upperbody upperbody ; 
Lowerbody lowerbody ; 


int’ valid; 

FastrakClass *fastrak_unit; 
FSTK_ stations torso_sensor; 
FSTK_stations upperarm_sensor; 
FSTK_stations lowerarm_sensor; 


FSTK_stations hand_sensor; 


// Fastrak related coordinate systems 


DrMatrix H tx to ts, Hotx toe vas; Hotx_toxlas, Hot ceeeen-, 


// Calibration matrices 

DiMatris Hts to tinkO,. H_uas to _link20; 
PreMatrix Hh las to link21, H hs to_liank2d; 
pfMatrix H_ts_to_screen; 

pfMatrix H_ts_to_posit0; 

Picak x2OLrrset, yviotiset, z.offiset: 

PIGAE EPOSIE, Y.pOSILE, ZopeSit; 
ELOatax=Ger, Y_ ref, Z rer; 


// Graphical model related coordinate systems 
pfMatrix H_screen, HO, H20, H21, H24; 
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// Body part lengths 
float spine_shoulder_length, uarm_length, larm_length, hand_length; 


void outputHMatrix(pfMatrix H_mat) ; 


pubic: 


)G 


#endif 


Body (const char *cfig_filename) ; 
~Body () ; 

Vena rotate {double *); 

void rotate_increment (double *); 
void draw(); 

void reset(); 

Hii eomore() { return valid; } 
vord get all inputs (); 

int calibrate(); 

int set_joint_angles(); 

int calculate_joint_angles(double *); 
ijemser. link llength(int; float); 


iiessee joint displacement (int, float); 
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ee 
i 
es 
fel 
is 
Ae 
cp 
ff 


KEK KKKKKEKK KEK KKK KKEKK EKER KEK KEKE KK KEK KEKE KEE KK EK KK KK KKKE KE 


FILENAME: body.cc 

PURPOSE: functions for the Body class 
angles-only tracking technique 

AUTHOR: P F Skopowski 

DATE: 1 Mar 96 | 

COMMENTS: functions for the Body class 


Ske knee ck) tele at kites ine eink ei ke Ke de kk ee tee eee i ek oe ok ee ee se keh ke ke ke kk eink) eink 


#include <math.h> 
#include <iostream.h> 
fine lude “body .h" 


if 
i] 
‘el 
// 
fe], 


Function: Body(const char *config_filename) 
Purpese: constructor of the body type 
: creates and initializes FastrakClass object 
: uses fastrak.dat configuration file 
Returns: body class object 


www ww ww www we re i i 


Body: :Body(const char *config_filename) 


{ 


valid = FALSE; 
faserak Unit = NULL; 


7/7 Open COontiguration tile 
ifstream config_fileobj(config_filename) ; 
1£ {!eontidg_fileos3) { 
Gerr << “Error. Opening COnEIgUuraLion iia: " 
<< config_filename << endl; 
return; 


// initialize matrices & variables 
pfMakeIdentMat (H_tx_to_ts) ; 
pfMakeIdentMat (H_tx_to_uas) ; 
pfMakeIdentMat (H_tx_to_las) ; 
pfMakeIdentMat (H_tx_to_hs) ; 
pfMakeIdentMat (H_ts_to_screen) ; 
pfMakeIdentMat (H_ts_to_link0O) ; 
pfMakeIdentMat (H_ts_to_posit0O); 
pfMakeIdentMat (H_uas_to_link20) ; 
pfMakeIdentMat (H_las_to_link21) ; 
pfMakeIdentMat (H_hs_to_link24); 
pfMakeIdentMat (H_screen) ; 
pfMakeIdentMat (HO) ; 
pfMakeIdentMat ( 
pfMakeIdentMat ( 
pfMakeIdentMat ( 
x offset = 0.0; 


H20); 
Het) 
H24); 


io? 


body.cc 2 


y_offset 
ZASErset 


iH 
OQ Oo 
Oo © 


=e 


//initialize Fastrak 
fastrak_unit = new FastrakClass(config_fileob}j) ; 


if @@#astrak_unit->exists()» { | 

if (fastrak_unit->getState(FSTK_STATION1) ) 
Eorso sensor = FSTK_STATIONI; 

if (fastrak_unit->getState(FSTK_STATION2) } 
upperarm_sensor = FSTK_STATION2; 

if (fastrak_unit->getState(FSTK_STATION3) ) 
lowerarm_sensor = FSTK_STATION3; 

if (fastrak_unit->getState(FSTK_STATION4) ) 
hand_sensor = FSTK_STATION4; 


Vole =a tRUE 


// Function: ~Body() 
// Purpose: destructor of the body type 


Body : :~Body () 
{ 
if ((fastrak_unit != NULL) && (fastrak_unit->exists())) { 
delete fastrak_unit; 
factrak unit = NULL; 


// Function: rotate (double *angles) 
// Purpose: set upperbody joint angles 
we >: uses the passed in array of values 


void Body::rotate (double *angles) 


{ 
upperbody.rotate(angles); 
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// Function: rotate_increment (double *increment_angles) 
// Purpose: increment upperbody joint angles 
uv : uses the passed in array of values 


void Body::rotate_increment (double *increment_angles) 
{ 


upperbody.rotate_increment (increment_angles) ; 


W7 Seunectl on: sdraw {) 
// Purpose: draw the body in the proper position 


void Body: :draw() 

{ 
// determine where to start drawing the upperbody 
pfMultMat (H_screen, H_tx_to_ts, H_ts_to_screen) ; 
pfMatrix temp; 
pfMultMat(temp, H_tx_to_ts, H_ts_to_posit0); 


MeoOmrpcet = CeMnp Lots): — H-ex—-to_ es (Os; 
Veorrset = templli(3] = Hlex to_ts([1) (3); 
z_offset = temp[2][3] - H tx _to_ts[2] [3]; 
x posit = H_tx to_ts[0] [3] - x_ref + x2@Biset; 
Meposit = H_Ex to ts[1](3] =v relies yecricen; 
Z posit = Hetx_tov2es{[2](3] - Ziserwe  zeerrsec; 


// set the openGL matrix (ie. array) 
GLfloat H_body [16]; 
H_body[0] = H_screen[0] [0]; 


H_ bedy(1] = H_screen([1) [0]; 
H body (2) = H-screen(2)) (0); 
hepody [3s] = H sereen(s) [0]: 
H_body [4] = H_screen[0] [1]; 
H_ body{5]) = H_screen([(1) [1]; 
Hepody{[o)] = Hi screen( 2) (1); 
H_body [7] = H_screen[3] [1]; 
H_body [8] = H_screen[0] [2]; 
H_body(9] = H screen([1] [2]; 


H_body[(10] = H_screen([2] [2]; 


H_body[(11) = H _screen([3] (2); 
H_ beedy [12] = x_posit; 
Hebecy i113] = yYopesit; 
H_body[(14] = z_posit; 
Hebedyi15 |] y= Hescreen(3] 13]; 


glMatrixMode (GL_MODELVIEW) ; 


glPushMatrix(); 


// align the tx and screen coord systems 
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glRotated(90.0, 1.0, 0.0, 0.0); 
glRotated(-90.0, 0.0, 0.0, 1.0); 


glMultMatrixf (H_body) ; 
upperbody .draw(); 
glPopMatrix(); 


// lowerbody.draw(); 


// EUneceion.: reset () 
// Purpose: reset upperbody joint angles 


void Body::reset() 


{ 
upperbody.reset (); 


// Function: set_link_length(int link, float length) 
// Purpose: set a specified link's length 


77 >: used to size the link to the user 
// Returns: TRUE if successful 
1 ecietatntiateteatetatahatatatataatamtataiaaaaiaiaataiatatatabatatabatatatntatntatatatetatatetatatatetatetaten 


timeweecwe set. link Jengen{int link, float length) 
{ 
if (upperbody.set_link_length(link, length) ) { 
heeirm CRUE; 
) 
return FALSE; 


// Function: set_joint_displacement (int link, float length) 
// Purpose: set a specified link's joint displacement 


7 "VWsea te size the link to the user 
// Returns: TRUE if successful 
[ [errr c ccc cr error ee 


int Body: :set_joint_displacement (int link, float length) 
{ 
if (upperbody.set_joint_displacement (link, length) ) { 
Fenurn LRUE, 
} 
return FALSE; 
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// Function: get_alli_inputs () 
// Purpose: get inputs from the fastrak trackers 


vi : Called to copy latest sample from second buffer 
i : implemented for double buffering to reduce 

a : lock overhead 

ad ; Called once at the beginning of each frame 


// Comment: original interface design by Scott McMillan 


void Body::get_all_inputs() 
{ 


if (fastrak_unit->exists()) { 
fastrak_unit->copyBuffer(); 


fastrak_unit->getHMatrix(torso_sensor, H_tx_to_ts); 

fastrak_unit->getHMatrix(upperarm_sensor, H_tx_to_uas) ; 
fastrak_unit->getHMatrix(lowerarm_sensor, H_tx_to_las) ; 
fastrak_unit->getHMatrix(hand_sensor, H_tx_to_hs) ; 


P7 FUNCEVOn: OUEDUE 
// Purpose: output homogeneous transformation matrix (4x4) 


void Body: :outputHMatrix(pfMatrix Hmat) 
{ 
fom werne 1-05. 1<4)a44) 
PEINngr(  socot so 3h veo.s fr Soins pita 
Hmat({ij[0], Hmat(iJ] [1], Hmatlil (2), @matiil ial: 
Prince vn) 


// Function: set_joint_angles () 
// Purpose: Set the body's joint angles using fastrak data 
// Returns: TRUE if successful 


int Body: :set_joint_angles() 
int valid = FALSE; 
double angles[25]; 
Pome Oi Zoe i++) { 
angles (aj. — 0.0. 
} 


valid = calculate_joint_angles (angles) ; 
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if (valid) { 
rotate(angles) ; 
} 


return valid; 


// Function: calculate_joint_angles(double *) 
// Purpose: calculate inverse kinematics 


// >: return the joint angles 

Vi : get_all_inputs must run first to update data 

// Returns: TRUE if successful 

ff mm nm nr rn rrr ener crc 


int Body: :calculate_joint_angles(double *angles) 


{ ; 
int valid = FALSE; 


double thetalsg 
double thetal9 = 
double theta20 = 
double theta21 = 
double theta22 = 
double theta23 = 
ecuble theta24 = 


if) 
@) (2 ( (eo) (ea {e@ fe 


° =e 


=e 


=e 


2) (SB) @ @] wwe 
~ 


“eo 


Gense Gouble deg_to_rad = .017453292519943295; 


HE (fast rak wunit->exists()) { 
7 onesteecall get all _inputs({) first 
Vallars TRUE; 


// convert reported data using calibration matrices 
PeMuleMat vO, Ho tx to ts; Hts to _link0O); 

DreMibbetat (20>) Hetxesto uas, Huas to _link20); 
pfMultMat (H21, H_tx_to_las, H_las_to_link21); 
pfMultMat (H24, H_tx_to_hs, H_hs_to_link24) ; 


yy eompuce lf. l7etee70 
piMatrix T_17_to 20, H_temp; 


eae aioe eemOe—9 440.0, 1.0, 0.0, 7.5}, 
fio M060 01.05 4020" 7 
oe 0. 0,, 050, 50.0 4, 
WoO ee O05. 020,70 tesOssj) > 


DIMatrix HO_inv; 
pfinvertFullMat (HO_inv, HO); 


pfMultMat (H_temp, HO_inv, H20); 
DrEMumeMet(Tolyote.20, T_17_to_0, H temp); 
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// Gee tneecaca Erom T217_to_20 
double a2 ely ete Ome Oe Os). 
ae lerae—sely co 20 Pili fl); 
double c3 elven. O12) [2] 3 
Gemiele c2 >=) Pel? -to-20(1) [2]; 
Gemble cl’= T_17_to 20[0)] [2]; 


// compute the sin of thetal9 
double sin_thetal9 = sqrt(a2 * a2 + b2 * b2); 


// check fer zero 

Tt (Sin _thetalo < 0-001){ 
Sin_thetal9 = 0.001; 

} 


// set the sign of the answer 
if (cs 2.0) { 

Sinetnecals ==] —1.0; 
} 


// compute the angles 

thetal9 = atan2({(sin_thetal9, c2); 

theta20 atan2 (b2/sin_thetal9, -a2/sin_thetalQ); 
thetal8 = atan2(c3/sin_thetal9, cl/sin_thetal9Q); 


Vf Compute To202to_ 2] 
pueMatrix iTe20Nren21, HZ0linv, 


DPrinvererullMarwi20_inv, H20); 
PEeMuleMae (1 20 ce 21, HZ0 inv, H21)> 
// get the data from T_20_to_21 
Pieat -a3 (= 1220 -boe7ia | oe 


Eloatybs =—sreZ20. tos2 2 iii 


// compute the angle 
theta21 = atan2(a3, b3); 


BeMacrix T2160 24 ,..H21. inv: 
BiinvereceuliMat (H21-inv, 421); 
Dima tMae(rE 21 to_24, H2l2inv, H24); 


// get the data from H24 
ace eee co 24 (2) [0 


f 


Seu eee. 242) (1); 
eo 
C2 =) deez ieee 241} (2); 


« 


f 


] 
] 
Tee eee om 4 [2 |) (2): 
] 
] 


Cie= leet eitoe2A (0) (2 
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// compute the sin of theta23 
Semele sim thetaz3 = -sqrel(as * a3 + b3 * b3)}; 


// check for zero 
tieetcin tCheta23 > —0.001){ 
Sinecnetaz2s = -0.001; 


// compute the angles 

theta23 = atan2(sin_theta23, -c3); 

theta24 = atan2(b3/sin_theta23, -a3/sin_theta23); 
PhebaZ2e= abtan2 (-c2/sin eneca23, -cl/sin_theta23) ; 


// convert all angles to degrees 


Enctale /= deq to_rad; 
EMetals /= deg to_rad; 
theta20 /= deg_to_rad; 
theta21 /= deg_to_rad; 
Eiheta22 —/= degq_to_rad; 


theta23 /= deg_to_rad; 
theta24 /= deg_to_rad; 
angles[{1l] = O20; 
angles[{2] = -90.0; 
angles[3] = C2U; 
angles([4] = 90.0; 
angles[5] = 9020+ 
angles[6] = 180.0; 
angles[7] = Oey 
angles[8] = OO; 
angles[9] = 90.0; 
angles[10] = Seo 
angles[1l] = G20; 
angles[{12] = Oa. 
angles([il3s] = CU: 
angles[14] = -90.0; 
angles[15] = C20: 
angles([lG) = 180.0; 
angles[17] = 0.0; 
angles[18] = thetalé8; 
angles[19] = thetal9; 
angles[20] = theta20; 
angles[21] = theta21; 
angles[22] = theta22; 
Srigtkes|(25 |) = ‘Eheta23 ; 
angles[24] = theta24; 


} 


return valid: 
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// Function: cali@rate () 
// Purpose: size the upperbody model to the user 


as : calibrate the trackers 
// Returns: TRUE if successful 
|b mmr rrr rrr ccccn-- 


int Body::calibrate() 
{ 
int valid = FALESE; 


pfMatrix H_torso_reported, H_uarm_reported; 
pfMatrix H_larm_reported, H_hand_reported; 


pfMakeIdentMat 
pfMakeIdentMat 
pfMakeIdentMat 
pfMakeIdentMat 


H_torso_reported) ; 
H_uarm_reported) ; 
H_larm_reported) ; 
H_hand_reported) ; 


ee ee 


i= (fastrak_unit->exists()) { 
valid = TRUE; 
Ehlat ctr: 


// request upperbody dimensions 
cerr << endl << "Input spine to shoulder length (cm): "; 
cin >> spine_shoulder_length; 

Gerr << “Input upper arm Tength: ”; 
Cin >> uarm_length; 

Cerr =< “Input lower arm. erecn: =: 
Cin. >> larmlengen: 

Gere << "Input Hand length: ”; 

cin >> hand_length; 

Gin.G@et (Str); 


// set upperbody dimensions to that of the user 
set_link _lengith (3m .0); 

set_link_length(6, 0.36 * spine_shoulder_length) ; 
set_joint_displacement(16, 26.0); 
set_link_length(17, 0.64 * spine_shoulder_length) ; 
set_link_length(8, 0.64 * spine_shoulder_length) ; 
set_link_length(20, uarm_length) ; 
Setalinkelengeh(11, uarm length); 
set_link_length(21, larm_length 
a2 
(24 
‘ols: 


/ 


) 

je 

lee 

set_link_length larm_length) ; 
set_link_length ) 
) 


set_link_length 


f 


hand_length 
hand_length 


/ 
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Gerr << end! << “Galabrating sensor orientation in 3 seconds..." << endl; 
cerr << "Press <Enter> to start count-down: "; 
Gin. get (str); 
fOr (int i=O;a@m<3; i++) { 
sleep(1); 
Gern << (char) 7; 


7/7 this code allows the fastrak to do the calibration for torso 
Vy iloeae sang les(3) = {90.0, —-90.0, 180.0); 
//fastrak_unit->setBoresight (torso_sensor, angles); 


// get the data to compute the calibration matrices 
fastrak_unit->copyBuffer(); 
fastrak_unit->getHMatrix(torso_sensor, H_torso_reported) ; 
fastrak_unit->getHMatrix(upperarm_sensor, H_uarm_reported) ; 
fastrak_unit->getHMatrix(lowerarm_sensor, H_larm_reported) ; 
fastrak_unit->getHMatrix(hand_sensor, H_hand_reported) ; 


// compute the calibration matrices 


// compute torso sensor calibration matrix 
pfMatrix H_torso_reported_inv; 


Piatt rix Ho teidesired = {{ 1.0, 0.0, 0.0, 0.0}, 
(07 Open CHeOmenm ONG } , 
{04010 0) els One 
(009 OO OnOret 073. 


pfInvertFullMat (H_torso_reported_inv, H_torso_reported) ; 


pfiMultMat (H_ts_to_linkO, H_torso_reported_inv, H_ts_desired) ; 


Beier xe H ts cesired2? = {{ 1.0, 0.0, 0.0, 0.0), 
e020 Se OS. YOR0e O20} 5 
100s are Or lh 9 epee © Bee © i Be 
P Oe Oe Uae, ales O i aie es Os) ap ae 


pfMultMat (H_ts_to_screen, H_torso_reported_inv, H_ts_desired2) ; 
PmoeeMaccol (hobs storscreen, 3,-0.0, 0.0, 0.0, 1.0); 


// compute torso sensor calibration matrix for positO tracking 
// some necessary matrices 

evar rdseeRwexebomes, R_ts_to_tx; 

pfCopyMat (R_tx_to_ts, H_torso_reported}) ; 


// set posit col to zero to work with rotation matrix only 
Motecelaeeomne ex FCOLES, 3, 0.0, 0.0, 0.0, 1.0); 
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// get the inverse rotation matrix 
pfTransposeMat (R_ts_to_tx, R_tx_to_ts); 

// determine suitable offsets from ts and linko 
Ploat seoreseE,= 0.0; 

float y_offset O.0; 

Lleat. gaeureeer—— 47.10; 


i) 


Jf CNC COELSete et FoOmeeseLloolinkO in world coordinates 


PEMAbGuieemontececemecneomlinkg? = {{ 1.0,. 0.0, 0.0, X [Offset 
{ 020, 2810 |. ee y_offset}, 
{ 02.07 OCy ole Oe z_ offset}, 
{0 . Ope OOo Oe 


pfMatrix temp; 

DiMuPpeMat(bemp, K_ts toetx, P_Ortset ts toa. linko) 

piSetMatCol (HLGseto_posito, 3, temp [0] [3], temp([1] (23 ene ieee oO): 
x ref = H torso_reported|(0] [3]; 

Verel = H torso; eported(1)] [3]; 

Zatel = HebOrnsoureporeced | 2) (3 |; 


// compute upper arm sensor calibration matrix 
pfMatrix H_uarm_reported_inv; 


peMatrix He@w@aem desired = {{' 0.0, 0.0, -1.0, 0.0), 
{0205 j..0eueO bo Oo) 
CodesOs OO, Ore OL Ora 
CAOwOs. 20c-07 OO 1Oaals: 


pfinvertFullMat (H_uarm_reported_inv, H_uarm_reported) ; 
pfMultMat (H_uas_to_link20, H_uarm_reported_inv, H_uarm_desired) ; 
// compute lower arm sensor calibration matrix 


pfMatrix H_larm_reported_inv; 
// place elbow in 90 bend 


PiMatrix Ho lam desired = {{ 1.0, 0-07 9050 30.07, 

{ O20, -0.03--=1..0)S07.077 

{ 0.07 2207720270; 3070e 

{ 0.05, “020. RCe soe. 
// was previously this, for straight down 
/7PpoeMatuix HH larm desired = {{ 0.0, -1.0, 90.0, .0-0)7 
igre { 0,;0,9 "020, S170 oe) 
fi { 12303 20.2055. 0.07 Oe Oe 
as { 0.07. -7020,.0 020 2 one 


pfinvertFullMat (H_larm_reported_inv, H_larm_reported) ; 
pfMultMat (H_las_to_link21, H_larm_reported_inv, H_larm_desired) ; 


// compute hand sensor calibration matrix 
pfMatrix H_hand_reported_inv; 
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// place hand straight out with elbow bent 


peMatrix H handme@esiread = {(°0.0, O20) 1.0, 0.0}, 
foiecovmmeo- 0, ) 0.0, 0.0}, 
Ome 205.6 0-0, 0:0), 
emcee,” 0.0, 120)); 

// was previously this, for straight down 

Prpemieerix Ho hanemaestred = (f 0.0, -1.0, 0.0, 0.0}, 

i, Poorer 070 0. 0,,°'0:0},, 

// Cevecrmoe OF 91.0, 0.0}, 

// eure mene 0, - O.0,°1.0}); 


pfInvertFullMat (H_hand_reported_inv, H_hand_reported}; 
pfMultMat (H_hs_to_link24, H_hand_reported_inv, H_hand_desired) ; 
} 


return Valid: 
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FastrakClass.h 1 

ha Se et a a a a a a ae oe a ee ee i ee ee ee ee a oe ee ee i ee ie ee 
// File : FastrakClass.h 

V7 AUCNOr 2 7Sseobe Mela ran 

ae : Naval Postgraduate School 

id : Code CSMs 

pi : Monterey, CA 93943 

id : memillan@cs.nps.navy.mil 

// Project : NPSNET - Individual Combatants/Insertion of Humans into VEs 
// Created : August 1995 

// Summary : This file contains the declarations for a C++ class to 

ai : Manage the Polhemus 3Space Fastrak. 

Vee. 

a For detailed information on the operation of the Fastrak, 
lave refer to the 3SPACE USER’S MANUAL. 

ii 

at This program was based on the ISOTRACK program written by 
i Paul T. Barham in Sept. 1993 for single sensor case. Major 
ag modifications have been made to adapt to multiple sensor 

i case. The resulting code, written by Jiang Zhu in 

ae Jan. 1995, underwent another major modification to support 
// binary data in continuous mode. 

Hag | 3 He. fad Pe OT CTO Cee eee ee ee Me eh de at dt ae MG Me Se De ee eg Oe hs Od To cup Cod Cot tah Tall Co Cad Cod Cod Cad Cod Caltad Cod Cal ah Cod Cad Cad Tad ah Cad Lad Cad eS VP GS US Tad Tad CaS Cad 


Copy rid: (CC) al995, 
Naval Postgraduate School 
Computer Graphics and Video Laboratory 
NPSNET Research Group 
npsnet@cs.nps.navy.mil 


Permission to use, copy, and modify this software and its 
documentation for any non-commercial purpose is hereby granted 
without fee, provided that (i) the above copyright notices and the 
following permission notices appear in ALL copies of the software 
and related documentation, and (ii) The Naval Postgraduate School 
Computer Graphics and Video Laboratory and the NPSNET Research Group 
be given written credit in your software’s written documentation and 
be given graphical credit on any start-up/credit screen your 
software generates. 

This restriction helps justify our research efforts to the 
sponsors who fund our research. 


Do not redistribute this code without the express written 
consent of the NPSNET Research Group. (E-mail communication and our 
confirmation qualifies as written permission.) As stated above, this 
restriction helps justify our research efforts to the sponsors who 
fund our research. 


This software was designed and implemented at U.S. Government 
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* expense and by employees of the U.S. Government. It is illegal 
* to charge any U.S. Government agency for its partial or full use. 


* THE SOFTWARE IS PROVIDED “AS IS” AND WITHOUT WARRANTY OF ANY KIND, 
* EXPRESS, IMPLIED OR OTHERWISE, INCLUDING WITHOUT LIMITATION, ANY 
* WARRANTY OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE. 


* E-Mail addresses: 


: npsnet@cs .nps.navy.mil 

- General code questions, concerns, comments, requests for 
“3 distributions and documentation, and bug reports. 

* npsnet-info@cs.nps.navy.mil 

= Contact principle investigators. 

i Overall research project information and funding. 

= Requests for demonstations. 


* Thank you to our sponsors: ARL, STRICOM, TRAC, ARPA and DMSO. 
oe 


#ifndef _ NPS_FASTRAK_CLASS__ 
#define __ NPS_FASTRAK_CLASS__ 


#include <ulocks.h> 
#include <fstream.h> 


// Boolean values 
#ifndef TRUE 
#define TRUE 1 
#endif 


#ifndef FALSE 

#define FALSE 0 

#endif 

Sonce eine LOREONAME SIZE = 48; 


// Assumptions and limitations which influence the use of the FASTRAK 
// program: 


ee 

// 1). I assume that the user of the FASTRAK program will use it ina 

vf Single process, say the application process in PERFORMER, that 

bi is, a instance of the ”“FastrakClass” class will only be used in a 
a process in which it is created. Hopefully, this is not too 

if restricted. 

a 

as The problem with the current version when it is used in multiple 

ia processes is that the read_data(), read_posorient() and 

sy read_homomatrix() methods are not guarded with the 

// data-record-parameter-updating binary semophere. Instead, a lock 
Ly is used for guarding data buffer switching. Refer to the 

// implementation for details. 

ay, 

// It is easy to fix this problem. Basically, what you need to do is 
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Li to guard the two methods with the above binary semophere. The 


L. COSE as thatesnow the 

// read_data()/read_posorient()/read_homomatrix() method and the 

if get_packet() method are almost totally mutually exclusive, which 
ia may Slow the system performance down. If this is done, the lock 
// for guarding data buffer switching can be eliminated. 

ae 

// 2). The FASTRAK program was witten so that it can be used to process 
// any number of sensors. The only thing you need to do is to change 
// the constant FSTK_NUM_STATIONS to the number of sensors you 

(As have. However, there is a limit caused by the system on the 

ey number of sensors you can use. Basically, the problem is that the 
hee data buffer size, BUFFER_SIZE, is constrained by the size of the 
Weds largest one-dimensional array allowed by the system. 


// Conventions used in this™@ile and FastrakClass cc: 


es 

i] All the constants and data types intended to be used by the user 
i of the FSTK start with the prefix FSTK. Those that do not have 

i this prefix should be used only in this file and FastrakClass.cc. 
fal 

// All constants are in capital letters. 


J// Terms used in this file amd Fastrak@lass .cc: 


Age 
gs Station: Each trasmitter and receiver pair is called a station in 
of the 3SPACE USER’S MANUAL. 


iy The algorithm: 


a 

// In single process mode, a single process, which is the one which 
ei creates the the “FastrakClass” object, requests a required type of data 
if packet from the FSTK when it needs one. 

a 

as In multiprocess mode, the process which creates the ”"FastrakClass” 
Li object spawns a light weight child process which continuously 

ee polls the FSTK to get a required type of data packet, which is the 
if data producer and runs in parallel with the parent process, the 

vf data consumer. 

as . 

ai The data packet is decomposed in the parent (or single) process, 
y/ the consumer, to generate the required type of data when a data is 
“ae requested by the user of the FSTK. 


// the masks used to specify the types of data requested from the FSTK 


#define FSTK_COORD_MASK ob*d aol // Cartesian coordi@iate (x, Y,2 
#define FSTK_EULER_MASK 02x0102 // Euler angles (Azim, Elev, Roll) 
#define FSTK_XCOS_MASK 0x004 // X-axis directional cosines 
#define FSTK_YCOS_MASK 0x008 // Y-axis directional cosines 
#define FSTK_ZCOS_MASK Ox010 // Z-axis directional cosines 
#define FSTK_QUAT_MASK 0x020 // QOvwaternion (Wak, YY, Z) 


#define FSTK_16BIT_COORD_MASK 0x040 // 16BIT format coordinate data 
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#define FSTK_16BIT_EULER_MASK 0x080 // 16BIT format euler angles 
#define FSTK_16BIT_QUAT_MASK 0x100 // 16BIT format quaternion 
#define FSTK_CRLF_MASK GCxZ200 // the framing CR/LF characters 
#define FSTK_DEFAULT_MASK O240.c0 // 16BIT COORD and EULER_MASKs 
// The sizes of the types of data returned from the FSTK -- in bytes 


// IEEE single precision floating point format has 4 bytes per number 


// 16BIT obviously has two bytes per 


#define 
#define 
#define 
#define 
#define 
#define 
#define 
#define 
#define 
#define 
#define 


FSTK_HEADER_SIZE 
FSTK_COORD_SIZE 
FSTK_EULER_SIZE 
PethexncOs.SiZe 
PST YGO@S_SIZE 
FPSTKEZCOS_SIZE 
FetReQuaAT_Size 
BSTK. SBI 7_CCORD_S 
Forks 6BI7_EULERES 


6 
Vote oott QUAT_SIZE 8 
2 


POURSGREPZSIZE 


3 
2 
a2 
AZ 
a2 
dz 
16 
IZE 6 
IZE 


number. 

data record header from the FSTK 
Position coordinates 

Euler angles 

X-axis directional cosines 
Y-axis directional cosines 
Z-~aXis directional cosines 
Quaternion 

16BIT format coordinates 
16BIT format euler angles 
16BIT format quaternions 
Carriage Return, Line Feed 


fae 
iy 
// 
ee 
// 
hi 
// 
iy 
// 
E 
// 


// There can be up to 4 stations active at the same time in the FASTRAK. 
#define FSTK_NUM_STATIONS 4 


#define MAX_PACKET_SIZE 
#define BUFFER_SIZE 


val 


// All of the above summed together 
364 // MAX_PACKET_SIZE*FSTK_NUM_STATIONS 


// 16BIT format requires scaling information for the various types of 


iy Gata: 


#define 
#define 
#define 
#define 


Ped tobi Tt TOlcM 
PSUKeal6BIT_TO_INCH 


Bo lle bi T TOSDEGREES 


Polke bob iT _TO-QUAT 


// Station numbers 
enum FSTK_stations 


{ 


// The FASTTRACK can return 


#define 


FSTK_STATIONI = 
PSTKZSTATIONG, 
PSTK_STATIONS, 
FSTK_STATION4 }; 


0, 


FSTK_NUM_DATATY PES 


enum FSTK_datatypes 


PerPSTRSCOGRD OOTY PE 
PST ReEUE ERT 2E, 
Beth ACOs =TY PE, 
FsTK_YCOS_ Tm, 
FSTKEZCOSLTYPE, 
FSTKZQUATSTYPE, 


(S00R07 b197) 


ES 


Gis ye192 ) 
(Tece oer) 


PPeO7oio2) 


// This may be bad programming style, 


but 


// these are used to index into arrays. 


up eo 
9g 

= 0, Lf 

Bs 


ForhReieslTeeGCRe TYPE, 
PSTReUSBIT_BULERITY PE, 
PSTK_l6Bi paul aris ); 


6 different types of data. 
// number of data types 


This may be bad programming style, but 
these are used to index into arrays. 
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// units used to measure the FSTK positions 
enum FSTK_units 
{ FSTK_INCH, FSTK_CENTIMETER }; 


class FastrakClass 
{ 


private: 
Iaye per eanc: // the serial port file descriptor 
char port_name[{PORT_NAME_SIZE] ; // the name of the Fastrak port 


// info for the individual byte buffers for the four Fastrak stations. 
char datarec{FSTK_NUM_STATIONS] [MAX_PACKET_SIZE] ; 
char datarec_buf [FSTK_NUM_STATIONS] [MAX_PACKET_SIZE]; 


// data records for each station 
short max_datarec_size; // size of the largest station pkt 
short datarec_size[FSTK_NUM_STATIONS]; // data rec. size for each station 


short datatype_mask[FSTK_NUM_STATIONS]; // data types for each station 
short datatype_start [FSTK_NUM_STATIONS] [FSTK_NUM_DATATYPES ] ; 


// the position of each requested 


: Wy type in che daratueceud 


short fstk_packet_size; // the sum of the data record sizes 


Vo, for ail the active stations, i-e., 


ae the size of a complete data 

tT packet returned from the Fastrak 
char read_buffer[BUFFER_SIZE] ; // pollcontinuously’s temporary buffer 
unsigned int read_index; // currene 1loGcaticonein temp Outrer 


// Process ID and process for the serial port polling process. 

int pareo! | aead 

friend void pollContinuously (void *); // Utilem@spree cavten to read port 
void getPacket(); // read a packet from the FSTK 


// locks and binary semaphores for ensuring mutual exclusive access 
// te Cultical seceions: 

// Note that a boolean flag, param_setting, is used together with the 
// semaphore. Basically, when the consumer process is already 

// holding the binary semaphore, it should not request the semaphore 
// again. Otherwise, deadlock would occur. It is needed when a 

// consumer data record parameter setting method needs to call 

// another such method. Now there is one such case, set_state() 


// calling specify_datatype(). Note that param_set_flag should never 

// be used in the data producer process. 

WleeGkat Gacralock: // a lock used to guide switching data buffers 

usema_t *paramsema; // a binary semaphore used to guide setting data 
Nas packet parameters. 

usptr_t *arena; // arena used to create lock and semaphore 
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// Boolean flags 
int param_set_flag; // TRUE if the data packet parameters are belng set. 


ite valida flag; // TRUE if initializing the FSTK is successfull 
meers Dolling_flag; // TRUE if the papallel pollamg process is not 
Ti being suspended 
volatile int data_ready_flag; // TRUE if new data has been read 
if after control parameter update 
Volatile int kill flag; // TRUE if exiting the parallel polling 
i7 process has been requested 


// Fastrak state variables: 


int active_setting [FSTK_NUM_STATIONS]; // h/w switch settings 

int active_state[(FSTK_NUM_STATIONS]} ; // s/w setting 

float alignment [FSTK_NUM_STATIONS] (3][3];// pp. 42-50 in User’s Manual 
float boresight [FSTK_NUM_STATIONS] [3]; lap pe OL = 55 

float hemisphere(FSTK_NUM_STATIONS] [3]; // pp. 88-92 

FS@K nits units; // CENTIMETERS and INCHES 


// private methods 


void initState(); // init the member variables 
imem——ecaaConrlg(lfistream &cemiiq_fileob ]); // read the config file 
int openlIOPort(); // open the FSTK serial io-port 
ine lil cma elprecessing () ; // initialize multiprocess mode 
int checkState(); // check which station is active 
void prepareToRead(); // parallel/serial i/f to getPacket 
int sendCommand(char* command, // send a command to the FSTK 

int lengtly, 


char* source); 


VO1d convertData(char* data, 7/7 GOnvert IEEE buffer data ta n 
ime num tloats, // floats: DOS ordered bytes to 
float data_dest[(}); // Unix (reversed) 

void convertié6éBITData(char* data, // convert a 16BIT format buffer 
int num_floats, /{/7GO mm JEEPer locating pointe nums 


floaie scale, 
float data_dest[}); 


// functions for debugging and error checking 
void debugData(char *data_store, int num_of_bytes) ; 
int detectErrer // detect errors in data packet 
VOld TreporesStavesrromichar* location, 
FSTK_stations station_num) ; 
int checkReadError(FSTK_stations station_num, char* source, 
FSTK_datatypes data_type) ; 


// Define what values are requested from the station. 


// Values requested should be ORed together using the mask. 
// Return TRUE if the operation is successful. 
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// See page 97-111, the 3SPACE USER’S MANUAL. 
int setDataTypes(FSTK_stations station_num, short mask); 


// Set the state of the station and return TRUE if sticcessful. 

// See page 128-131, the 3SPACE USER’S MANUAL. 

cy 

// Note that when the state update is from FALSE (INACTIVE) to TRUE 
if (ACT, aeomeea |.) 

// to setDataTypes() should follow the call to set_state() to 

// specify the data types. By default, FSTK_DEFAULT_MASK is used. 
int setState(FSTK_stations station_num, int active_flag); 


// alignment reference frame functions: 
void getAlignment(FSTK_stations station_num, 
Pleat Origin([3], 
Eleake x pointe (3), floatey_peint [3 ine 
int setAlignment (FSTK_stations station_num, 
comee float omicqini3 |, 
Conctatloat. x peine [oi 
const filieat yepoink | 3de; 
int resetAlignment(FSTK_stations station_num) ; 


// SOresicht tunceion: 
int resetBoresight (FSTK_stations station_num) ; 


// active hemisphere functions. 

void getHemisphere(FSTK_stations station_num, float zenith[3]); 

int setHemisphere(FSTK_stations station_num, const float zenith[3]); 
int resetHemisphere(FSTK_stations station_num); 


// position measurement units. FSTK_CENTIMETER is default. 
int setUnits(FSTK_units pos_units)  ; 
inline FSTK_units getUnits() const { 

retburn (units ys 


Dub lee : 
// This constructor expects the name of a configuration file for the 
// FASTRACK and a flag list indicating data types desired. 
FastrakClass(ifstream &config_fileobj,- 
short datatype_flags = FSTK_DEFAULT_MASK) ; 
~FastrakClass(); // saeSErUc cer 


// Return true if initializing the FSTK is successful. 
inline int exists() const { 
return valid_flag; 


// In multiprocess mode, suspend and resume the execution of the 
// parallel polling process, the data producer. During the 

// suspension, no new data is produced. Return TRUE if the 

// the is successful. 
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int Suspend (); 
vold resume () 


‘ 


yoeGet the state of the station. 
inline int getState(FSTK_stations station_num) const { 
return(active_state[station_num] ); 


// move data to second buffer (reduces lock overhead) 
void copyBuffer(); 


// Read the specified type of data from the specified station. 

// For a successful read, data_dest[}] contains the result. 

// Note that data_dest[] must be a 4-element array for quaternions; 

// for the other types of data, data_dest[] is a 3-element array. 

// Return TRUE when the read is successful. 

int readData(FSTK_stations station_num, FSTK_datatypes data_type, 
float data_dest[]}); 


// Read a homogeneous transformation matrix of the sensor with 

// respect to the transmitter. For a successful read, the upper left 
// 3x3 submatrix of matrix[][{] contains the rotation matrix 

// constructed from the quaternion, euler angles, or X-, Y- and 

// Z@-directional cosines (depending on which type was selected) of 
// the station which results in the X-cosine in the first row, 

// Y-cosine in the second, and Z-cosine in the third; if 

// FSTK_COORD_TYPE has been selected, the last column contains the 
7/7 @sSition Sf fhe station, otherwise, it is filled with 0. Rettirn 
// TRUE if the read is succesful. Otherwise, return FALSE. 

int getHMatrix(FSTK_stations station_num, float Hmatrix[(4][(4}); 


// Read the current position and orientation of the station together. 
// On a successful return, posit[] contains the position and orient([] 
V7/eeeonrtaine the GCriemeation of the station. The type of the 
// orientation, euler-angle and quaternion, is determined by 
// orient_type. Note that if orient_type is FSTK_EULER_TYPE, orient 
// is a 3-element array. Otherwise, it must be a 4-element array. 
// Return TRUE if the read is succesful. Otherwise, return FALSE. 
int getPosOrient(FSTK_stations station_nun, 

FSTK_datatypes orient_type, 

fioge poset, tloart orient ()); 


// Boresight functions 

void getBoresight(FSTK_stations station_num, float orient[3}); 

int setBoresight (FSTK_stations station_num, const float orient[3]); 
}; 


#endif 
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// 
de 
// 
as 
i, 
as 
// 
// 
id 
we 
of 
ae 
Lt 
// 
a 
a 
// 
ie 
ae 
aE 
as 
Ms 


KaeKK KK KKK KKK KK KEK KEK KKK KEKE EKER KEKE KK KEKE KKK KKK KEK KKK KK KEE KEKE KK KEKE KKK KKK 
File : Fasitrak@@ass.cc 
Author : Scott McMillan 
: Naval Postgraduate School 
Code CSMs 
Monterey, CA 93943 
: mcomillan@cs.nps.navy.mil 
Project : NPSNET - Individual Combatants/Insertion of Humans into VEs 


Created : August 1995 
Summary : This file contains the declarations for a C++ class to 
: manage the Polhemus 3Space Fastrak. 


For detailed information on the operation of the Fastrak, 
refer to the 3SPACE USER’S MANUAL. 


This program was based on the ISOTRACK program written by 
Paul T. Barham in Sept. 1993 for single sensor case. Major 
modifications have been made to adapt to multiple sensor 
case. The resulting code, written by Jiang Zhu in 

Jan. 1995, underwent another major modification to support 


binary data in continuous mode. 
TOK KKK KKK KK KK RR KK KR KKK KR Ke OK Ke eX Kee ee Keke Re ee Oe ok, ee ea ie a 


Copyright (c) 1995, 
Naval Postgraduate School 
Computer Graphics and Video Laboratory 
NPSNET Research Group 
npsnet@cs.nps.navy.mil 


Permission to use, copy, and modify this software and its 
documentation for any non-commercial purpose is hereby granted 
without fee, provided that (1) the above copyright notices and the 
following permission notices appear in ALL copies of the software 
and related documentation, and (ii) The Naval Postgraduate School 
Computer Graphics and Video Laboratory and the NPSNET Research Group 
be given written credit in your software’s written documentation and 
be given graphical credit on any start-up/credit screen your 
software generates. 

This restriction helps justify our research efforts to the 
sponsors who fund our research. 


Do not redistribute this code without the express written 
consent of the NPSNET Research Group. (E-mail communication and our 
confirmation qualifies as written permission.) As stated above, this 
restriction helps justify our research efforts to the sponsors who 
fund our research. 


This software was designed and implemented at U.S. Government 


expense and by employees of the U.S. Government. It is illegal 
to charge any U.S. Government agency for its partial or full use. 
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* THE SOFTWARE IS PROVIDED “AS IS” AND WITHOUT WARRANTY OF ANY KIND, 
* EXPRESS, IMPLIED OR OTHERWISE, INCLUDING WITHOUT LIMITATION, ANY 
WARRANTY OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE. 


+ 


* E-Mail addresses: 


* npsnet@cs.nps.navy.mil 

ys General code questions, concerns, comments, requests for 
: distributions and documentation, and bug reports. 

: npsnet-info@cs.nps.navy .mil 

2 Contact principle investigators. 

‘ Overall research project information and funding. 

. Requests for demonstations. 


* Thank you to our sponsors: ARL, STRICOM, TRAC, ARPA and DMSO. 
0 f 


#include <stdlib.h> mec standard library stuff 
#include <stdio.h> 

#include <math.h> 

#include <strime.h> 

#include <bstring.h> 


#include <iostream.h> // FormG++ stamdard 1/0 stuff 

#include <fstream.h> // For C++ file I/O stuff 

#include <unistd.h> // For standard Unix read, write stuff 
#include <errno.h> 

#include <fcntl.h> // For file constant definitions and flags 
#include <termio.h> // For terminal I/O stuff 

#include <termios.h> // For eerminal 1/0 stuff 

#include <sys/types.h> // For system type stuff 

#include <sys/prctl.h> // For process control stuff 

#include <sys/signal.h> // For process signal stuff 


#include "*FastrakClass.h” 
//#include “jointmods.h” 


// the file for creating the shared data arena used by parallel 
// processes. 
#define IARENA_FILE “/tmp/fastrak.arena.data” 


// the permission option to chmod command-to alter the permissions on 
// the arena file to be read and written by everyone. 
#define ARENA_PERMISSIONS 0666 


// other convenient constants 


//#define DEBUG aL 
#define BELL (ener) 7 


#define FSTK_X 
#define FSTK_Y 
#define FSTK_Z 
#define FSTK_AZ 


OnNrF Oo 


J/ azinuth 
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#define FSTK_EL il // elevation 
#define FSTK_RO 2 lige neon 


#define RTOD 180.0/M_PI 
#define DTOR M_PI/180.0 


Vy) Jecal “functions 


Seottcevolemrec iG handler({int sig, «..); 

a ae a a 
ay Function: réporte syserr 

i] Returns: 

// Parameters: 

yf Summary: Report system errors 

a ee 


BEotic VOld report syserrtecnar* Grr_info, char* locatrien) 
{ 
Cerr <ageeLL << "Error in.” << err-into <<) 21 
<< " Error Number in *FastrakClass.ce*” << lecabtene 3) 
<< Crrne <= "emis 
perror(” Error Message”); 
} // end report_syserr () 


ae a a es ee 
Woe Fumetion: polleCentinucus]y 
i] Returns: 
// Parameters: 
oe Summary: This is the data producer which is called in multiprocess 
ise : Mode in a process running in parallel with the one that 
y/ : Calls the constructor and most of other methods of 
Ef : “FastrakClass”. It continuously reads the Fastrak for 
i] ; data and stores the Gata in the dataree™ Tis preceos wie 
vie : until the “kill _ flag” is sét by the consumer. 
[ [ann nr nr rr rr rr rr recs 
vold poll@emtinucusly(weid* tracweetob) ect) 
{ 

Static Void (* Temps s tamale.) 

Fastmkaktlass *tracker = (FastrakClass*~)tracker-ob) cet, 


// The following call is necessary for-this child process to actually 
// respond to SIGHUP signals when the parent process dies. 


prctl(PR_TERMCHILD)  ; 


if ((Signal.{ SIGTERM, (void (*) (...)})fosigehandler 2 =-.c teres, 
perror(”FastrakClass:\tError setting SIGTERM handler -\n aia 


if ((signal ( SIGHUP, (void (*)(...))£_sig_handler )) == SIGoaRF) 
perror(”FastrakClass:\tError setting SIGHUP handler -\n ca ie 


1f ( (tempesignal = signal ( SIGINT, (void (*)(...-.)) f2sigehagie. 
V=SSiGewe i j 4 
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ieomersional ( SIGINT, SIG IGN )) == "SIG ERR) 
perror(”FastrakClass:\tError setting SIGINT ignore -\n nes 
} 
ZE ((Signal ( SIGQUIT, (void (*)(...))f_sig_handler )) == SIG_ERR) 


perror(”FastrakClass:\tError setting SIGQUIT handler -\n ie 


#if DEBUG 
cerr << “Fastrak polling process initiated.” << endl; 
#endif 


if (Wweiteitracker-sp@et_fd, *C”, 1) != 1){ 
report_syserr(”sending FASTRAK C command”, 
Ho cOnkLInucusly~ ); 
} 


while (!tracker->kill_flag) { 
tracker->getPacket(); 
} // end while loop 


WimiweleettGmacKker—->port fd, “c”, 1) != 1) { 
report_syserr(”sending FASTRAK c command”, 
“HOLL Coneinuously” ); 
} 


tracker->kill_flag = FALSE; 
#if DEBUG 
Cerr <28%Rastrmak polling process terminated.” << endl; 


#endif 


exit(0); 
} // end pollContinuously () 


ff mm ee nnn nn nena =- 
fy Function: f_sig_handler 
ee Summary : 
// Parameters: 
es Returns: 
ff mmr re rr rrr ec cccccne- 
vVold fecig handler(int sig, ~:..) 
{ 
if ((signal ( sig, SIG_IGN )) == SIG_ERR) { 
perror(”FastrakClass:\tSignal Error -\n ad a 
} 
switch ( sig) { 
case SIGTERM: 
CxIE (0); 
break; 
case SIGHUP: 
TPeiederppiaty == 1°) ( 
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Geri <<@bEATH N@PMICE:” <<mend!l; 
cerr << “\tParent process terminated.” 
<< endl; 
cerr << ”“\tFastrakClass terminating to prevent orphan process.” 
<< endl; 
exe 00s 
} 
break; 
case SIGINT: 
cerr << “DEATH NOTICE: FastrakClass exitting due to interrupt ” 
<< “Signal.” << endl; 
exit(0O); 
break; 
case SIGQUIT: 
Cerr << “DEATH NOTICE: wPastrakClass exitting due co quic ~ 
<< “Signal.” << endl; 
exit(0O); 
break; 
default: 
cerr << “DEATH NOTICE: FastrakClass exitting due to signal: %” 
<< Gig << endl; 


exe (0); 
break; 
} 
signal ( sig, (void “@arC...))eeetgehandler ); 
} 
//SSS SS SS SS SSS SSS SSS SSS SS SS SSS SSeS SSS SS SS SSS SSS SS SS SSS SSS SS SS SSSSSSssSsa 
// FastrakClass class methods 
Uf a a a a a a SS a a a a a a ee a 
een Reo = oe a — Se Se ee 
// Function: Fastrak€lass :mastrak@leass 
iad Relurns * 
77 Parameters : 
// summary: constructor for FastrakClass object 
ag ae aaa SS SSS a Se ORS SS a Se ee ee 


FastrakClass::FastrakClass(ifstream &config_fileobj, short datatype_flags) 
{ 


// Initialize instance variables. 
initState(); : 


// Read in the configurations and open the FASTRACK port. 
Gt ((valegetilag = readConfig(config_ti leos7)) === Thuan, 
valid flag = openlIOPort (); 


VE (Valiaetlad) { 
// the *Y command for resetting FASTRAK 
// Refer to pp 76, the 3 SPACE USER’S MANUAL for details. 


Seaelerenarmecommanal) = { Oxo); 
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cerr << “Initializing Fastrak to start-up state.” << endl 
ee, This takes about 10 seconds...”; 
if (write(port_fd, command, 1) == -1) { 
valid_flag = FALSE; 
report_syserr(”sending FASTRAK “Y command”, 
"FastrakClass::FastrakClass”); 
} 


else { 
sleep(2); 
fOimecime 129; 15-15 1-=) { 
Gene <a fo<< * *: 
sleep (1); 


} 


cerr << endl; 


tee abicderiagd) && (valid_flag = checkState())) { 
valid_flag = setUnits (FSTK_CENTIMETER) ; 
for (int 1 = 0; i < FSTK_NUM_STATIONS; i++) { 
if ((valid_flag) && (active_state[i])) { 
valid_flag = setDataTypes((FSTK_stations)i, datatype_flags) ; 


} 


TOrmwimerstation num = O; station _ num < FSTK_NUM_STATIONS; station_num++) { 
if ((valid_flag) && (active_state[station_num])) { 
valid_flag = setHemisphere((FSTK_stations) station_num, 
hemisphere[station_num] ); 
Peeivalrarriag) { 
valid_flag = setAlignment ((FSTK_stations) station_nun, 
alignment [(station_num] [0], 
alignment[station_num] [1], 
alignment [station_num] [2]); 


} 


// mcemillan - 950814 - new code to read IEEE binary format data 
fiw wo bidet lag) \{ : 
// Enable binary output format from the FASTRAK. 
// Refer to pp 114, the 3 SPACE USER’S MANUAL. 
To Seuerce (pOrGuid mak 3) 1) != Lye{ 
report_syserr(”sending FASTRAK f command”, 
"FastrakClass: :FastrakClass”); 
valid_flag = FALSE; 
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if (value@ilag) st 
// Inttwatze the multiprocess mode, i.e., creating the 1ocke 
// and semaphors and sprocing the Fastrak data producer process. 
Validstilag = initMulemerocessing(); 


} 


Qf (valves r lag) at 
is polling _tlag = TRUE; 
} 


#i1f£ DEBUG 
cerr << "The FASTRAK object constructed. \n’”; 
ie (Val Ve Beta oy 
cerr << “The FASTRAK intialization is successful.” << endl; 
} 
else { 
Cerr << "The FASTRAK intializatiomeis unsuccessful.” <<"enal- 
} 
#endif 


} // end FastrakClass::FastrakClass() 


et Kieteteteteitatettattetatatetentetatetatetatatatetetatetetatateetatatetatatatetatetetetatetetetetateteteteetatetetetateatetetete 
vy Function: FastrakClass::~FastrakClass 

ioe Returns: 

// Parameters: 

os Summary: destructor for FastrakClass class 

0 SSeS SO SSS SS SS eS SS a 


FastrakClass: :~FastrakClass() 
{ 
#if DEBUG 
Cerr <<" Fast rak. adestructor callede\n’-; 
#endif 


at (Veloce lag) { 
// In multiprocess mode, signal the producer pre@@ess tomdic- 
// Then, free the lock and semaphore. 


if (parpeitepra != — jae 
ke ied. = eRe: 
if (!is_polling_flag) usvsema(paramsema) ; 
while (kill_flag); 
sleep(1); 


Daboeelipid = =1; 


usfreelock (datalock, arena) ; 
usfreesema(paramsema, arena) ; 
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// Flush all characters from the serial port and then close it. 
Ee¢ilush (port mad, TCI@FLUSH) 
close (port_fqd) ; 
valid_flag = FALSE; 
} 
} // FastrakClass::~FastrakClass() 


Ca a 
// Function: initState 

fy Returns: 

// Parameters: 

if Summary: Initialize instance variables to their default states. 

bf mr me ee nn enn 


void FastrakClass::initState() 

{ 
bzero(read_buffer, BUFFER_SIZE) ; 
read_index = 0; 
max_datarec_size = 0; 


pakpollep va v= <= 1. 
param_set_flag = FALSE; 
is_polling_flag = FALSE; 
data_ready_flag = FALSE; 
kill_flag = FALSE; 


// Initialize hemispheres and alignments. 

// Refer to pp 42 - 50 and pp 88 - 92, the 3 SPACE USER’S MANUAL 

// for the default values. 

Pemeeliemcotraclouenim — O- Station num < FSTK NUM STATIONS; station _num++) { 
hemisphere[station_num] [FSTK_X] (ee oe 
hemisphere[station_num] [FSTK_Y] = 0.0; 
hemisphere[station_num] [FSTK_Z] = 0.0; 


poseciahe | station num) ([FSTK_AZ] = 0.0; 
boresight [station_num] [FSTK_EL] = 0.0; 
POresight (Station num] [FSTK_RO] = 0.0; 


WaecCElcins 


alilcument [Station num] (0) [FSTK_X] = 0.0; 
alignment [station_num] [0] [FSTK_Y] =-0.0; 
alichiment (station num] [0] [RSTK 7) = 0.0; 


// X directions 


alignment [station_num] [1] [FSTK_X] = 1.0; 
alignment [station_num] [1] [FSTK_Y] = 0.0; 
alignment [station_num] [1] [FSTK_Z] = 0.0; 


// Y directions 

alignment [station_num] [2] [FSTK_X] = 0.0; 
alignment [station_num] [2] [FSTK_Y] = 
alignment [station_num] [2] [FSTK_Z]) = 0.0; 


ps 
© 
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J//sinitwalize Gata record parameters. 
active_state[{station_num] = FALSE; 
GCatabecestze(sStati1eon num) = 0; 

bzero(datarec_buf [station_num], MAX_PACKET SIZE) ; 
bzero(datarec{station_num], MAX_PACKET_SIZE) ; 


for (int type_num = 0; type_num < FSTK_NUM_DATATYPES; type_num++) 
datatype_start [{[station_num] [{type_num] = -1; 
} 


fstk_packet_size = QO; 
} // end initState() 


8 ee a as a SS SS SS SSS 

ie Function: readConfig 

af Returns: TRUE if the read is successful. Otherwise, return FALSE. 

// Parameters: 

Lf Summary: Read the configuration file for the FASTRAK. Called by a 

vy »- *“FastrakGlascs” constructor to do initialization. The, should 
Aes : not be called elsewhere. 
ee 
#define MAX _CONFIGFILE_LINESIZE 255 

#define CONFIGFILE_COMMENT_CHAR #! 


int FastrakClass::readConfig(ifstream &config_fileobj) 
{ 

int success = TRUE; 

char tmp_str[MAX_CONFIGFILE_LINESIZE] ; 

int station_num; 


while (config_fileobj >> tmp_str) 
{ 
// When a comment char is read, skip the rest of the line. 
if (tmp_str[0] == CONFIGFILE_COMMENT_CHAR) { 
config_fileobj.getline(tmp_str, MAX_CONFIGFILE_LINESIZE) ; 
} 
else 1f (Strmecmp(tmp str, “PORT 7.94) 5=—=.0)) 3 
config_fileobj >> port_name; 
} 2 
else if (Strmemp(tmp_str, “WANTED STATI@NS 2c) =="C)'| 
int state; 
for (station_num = 0; station_num < FSTK_NUM STATIONS; 
Station_num++) { 
G6enfig fileobj >> state; 
active_state[station_num] = state; 
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else { 
char param_str[30]; 
ert as 3 


FOre int Staeron. NUM e—=—0,-eecac don num < FSTK_NUM_ STATIONS; 
station_num++) { 
sprintf(param_str, “STATION%d_PARAM”, station_num+1); 


de -{ 
if (tmp_str[0]) == CONFIGFILE.COMMENT_CHAR) 
config_fileobj.getline(tmp_str, MAX _CONFIGFILE_LINESIZE) ; 
elsenin (Strncmpe(tmp-str, param_str, 10) == 0) { 
char param _name[30]; 


config_fileobj >> param_name; 
tome i = 0; 1 < 3; 1++) 
config_fileobj >> hemisphere[station_num] [1]; 


BOE (i =O; 1 ees +4) rt 
config_fileobj >> param_name; 
Lond =O; J =< 37 J++) 
COniigsiileod; >> alignment [Station_num] (i) (3); 


Sone 61 leob)] +> CmMp.{Str; 
break; 
} 
else { 
success = FALSE; 
GeGG) <9 585 << “Error in’ reading config Lile.\n’” 
meee *FastrakClass .cc=rFastraxc lass: :reaqd@onrrg-” 
<< “ illegal string: * << tmp_str << endl; 


} 
pewotles(ecnrig Lileob] >> tmp_str); 


I 7/ endatoer 
yy rena: it 
} // end while 


#if DEBUG 
sel egret ye 
CGrr 2-8 * reéqqconrtig:\n” << ” RASTRAK pore: “ << port mame << “\n” 
cee Stations requested: "”; 


for (i = 0; i < FSTK_NUM_STATIONS; i++) 
Gerre<. active state(i) << * *; 
cerr << endl << endl; 


BOr (ies 0 > 4 <sr STK NUM STATIONS? i++} { 
€erbn << “STATION” << i+] << “_PARAM: \n’%; 
cerr << “ hemisphere:\t”; 

EOn (qe Os 7 er e+) 
cerr << hemisphere[iJ[j] << ” "; 
Seine 1 tl Ollie: vi > 
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Pomme 0); J <3; j++) 

Cores alignment (1) (Ola) << * *; 
cerr << “\n x@eeent:\t”; 
for Boe 0 ec + +) 

Germ << aligumenem |) ([1)[j] << * “; 
Seige —e) \ie ype imran (Gn, 
TOte l= 30 pee) ea Ske a) 

Gemige <ona tonmene ll) [2 )iiaience *~)": 
cerr << endl; 

} 


ecerr << endl: 


cerr << “FASTRAK configuration parameters read.\n’; 
#endif 


return (success); 
fey /send FastrakClass: :readCcontig() 


a  -— -—  eeeee  ee 
as Function: openIOPort 

a Returns: TRUE for a successful opening. Otherwise, return FALSE. 
// Parameters: 

i 7/ Summary: Open the FASTRAK io-port 

[ff mmr rr nn en enna 


int FastrakClass::openlIOPort() 


{ 
int success = TRUE; 


// Test to see if the FASTRAK if on. 
if ((port_fd = open (pomemname , 40 _RDWRWG.NONBROGK) )S== -1) 
Success = FALSE; 
report_syserr(”opening the Fastrak port”, 
“PastrakClass::openlIOPort”); 
} 
else { 
Gham commeanel|s |. butfter[100)];> 
Strepy (command, “li\r’); 


1=£ (write(port_fd, command, strlen(cémmand)) == -1)m=e 
success = FALSE; - 
report_syserr(”“sending FASTRAK 1 command”, 
"“FastrakClass: :openIOPort”); 


Sleep (1); 
i= p@lsilecess) Yaa {read(port_id, butter, 100) == —1) 34 
success = FALSE; 


report_syserr(“reading FASTRAK”, 
"“FastrakClass: :openIOPort”); 


167 


FastrakClass.cc j 2 


) 


eltese (port_fd) ; 


// Do a blocking read when polling the FASTRAK for data. 
if ((success) && ((port_fd = open(port_name, O_RDWR)) == -1)) ( 
success = FALSE; 
report_syserr(”opening the Fastrak port”, 
"“PFastrakClass::openIOPort”) ; 
) 
else if (success) { 


fae 
He 


struct termio term; 

memset (&term, 0, sizeof(term) ); 
term.c_iflag IXOFF; Ve hixMeE * / 
term.c_oflag U7 

term.c_cflag B96001CS8!|CLOCAL|CREAD!|HUPCL; 
beurre lklag = 0; 


Eerm.c.line = 0; 77 Delocl: 
Perse Ce lvMiN |= 0; 

Cemiweeecc(VTIME) = 5; 
PomivecriaiwOorheta, TCSBRK, 0) == -1) { 


success = FALSE; 
close (POrE fa); 
report_syserr(”sending a BREAK to the Fastrak port”, 
"Facerakclace: sopenlLOPort” ); 
} 
else if (ioctl(port_fd, TCSETAF, &term) == -1) { 
Ssiiccess "= FALSE; 
close (port_fd); 
report_syserr(”setting the Fastrak port parameters”, 
"FastrakClass::openlIOPort”); 


Just in case the fastrak was accidentally left in continuous mode? 
(success) { 
char data[100]; 
Dealt nor ; 
While (nbr =) read(port_ fd, data, 100)) > 0) 
//cerr << “Warning: cleared ” <<-nbr << “bytes from Fastrak buffer” 
re —<25 211) Open lOpemes ~<< endl: 
Weiee (poneerad, "C”,. 1)s 
//debugData(data,nbr) ; 
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if “(success i { 
if (tetlish® (pomt_fay TCRORBISH) == -») { 
success = FALSE; 
eliese (ponte fd); 
report_syserr(”flushing the Fastrak port”, 
“FastrakClass: :openIOPort”); 


#if DEBUG 
cerr << “FASTRAK io=port, ” << port.lname << ”, opened” << endl: 
#endif 


return(success) ; 
} // end FastrakClass: :openIOPort () 


ee ee = 
yi Function: checkState 

// Returns: TRUE if all the requested stations are available. 

i : Otherwise, return FALSE. 

// Parameters: 

a Summary: Check for the availability of the FASTRAK stations. 
a a ne os eee 


int FastrakClass::checkState() 
( 

int success = TRUE; 

char command([5]; 

char data[100]; 


// Construct the “1” command to Geb the States Cf tee corona. 
// Refer to pp 128 - 131, the 3 SPACE USER’s MANUAL for details. 
// Choose any station to get the states for all stations. 

Strepy (command; “li\r"); 


lf (wrike(port2id, command, strlen (@onmmand) === —1 ia 
success = FALSE; 
report_syserr(”sending FASTRAK 1 command”, 
"FastrakClass::checkState” ); 


// Find out which station is active by hardware configuration. 
if (success == TRUE) { 


// 951002 - mcmillan - IMPORTANT BUG FIX: 

// do raw tty processing to get the answer because on the faster 
// machines and especially the onyx platforms the read occurs 

// sooner than the data is ready. 

Const 1nt@NUe BYTES = 9; 

const int MAX _POLL_RETRIES = 100000; 

ine counmta =e. | 

ine mum tries = 0; 

A alee ong 
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while ((count < NUM_BYTES) && 
(num_tries < MAX_POLL_RETRIES) && 
((abr = reaqq (pone encrmaccaacal(count)], 1})) !=--1)) { 
num_tries++; 
count += nbr; 


if (count != NUMZJBYTES) { 
success = FALSE; 
if (num_tries == MAX_POLL_RETRIES) { 
cerr << BELL << “Error: too many retries reading fstk port\n’”; 
} 
else { 
cerr << BELL << “Error: fstk port ‘read failed\n’; 
} 


G@emree- slit *FastraxClass,ce*PastrakClass: :checkState\n’; 


#if DEBUG 
debugData(data, 9); 
#endif 
} 


1f (success == TRUE) { 
LOne(ine 1 = 90, 1 < FSTK NUM_STATIONS; i++) { 
active_setting[i}] = (data[i + FSTK_HEADER SIZE} == ‘1’); 


tie (aGelvessece nay [1}) 
if ('tactive_state[i]) 
setState((FSTK_stations)1i, FALSE); 
} 
else { 
it {aclivemecaceli)]) { 
success = FALSE; 
cerr << BELL 
<< "FiseOr 1M “SGULINng FASTRAK Station state. \n? 
<< “ in *FastrakClass.cc*FastrakClass: :checkState\n’” 
oa) Ss ObatlOn —oat wec< = Mceregquirea to be active. \n” 
<< “ However, it is not set to be active by the” 
<a Shanrcware switch .\n*; 
GebugData(data, 9); 


} 
} // end for 
} 


else { 
report_syserr(” reading FASTRAK”, “FastrakClass: :checkState”); 


#if DEBUG 


cerr << “The states of the FASTRAK stations checked.” << endl; 
#endif 
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} 


tad 
ht 
// 
gs 
ag 
a 
a 


return (success) ; 
// end FastrakClass: :checkState() 


eee ee eee 


Function: initMultiprocessing 
Returns: TRUE if the initialization is successful. Otherwise: 
: return FABSE. 
Parameters: 
Summary: Initialize for multiprocess mode, including getting locks 
: and semaphores and sprocing a child process for polling 
the FASTRAK. 


int FastrakClass::initMultiprocessing () 


{ 


int success = TRUE; 


// Create an arena file to get the needed lock and semaphore 
if ((arena = usinit(IARENA_FILE)) == NULL) { 
success = FALSE; 
report. syserrm( “getting an arena file”, 
“FastrakClass::initMultiprocessing” ); 
} 
else { 
// Set up the arena file with read and write permissions for 
// everyone. 
if (usconfig(CONF_CHMOD, arena, ARENA_PERMISSIONS) == -1) { 
success = FALSE; 
report_syserr(”configuring an arena”, 
"FastrakClass: :initMultiprocessing”) ; 


// Create a lock to provide mutual exclusive access to the data 
// putfers. Refer to getPacket() for more info. 
if (success && ((datalock = usnewlock(arena)) == NULL)) { 
success = FALSE; 
report_syserr(”creating a lock”, 
"FastrakClass: :initMultiprocessing” ); 
} 
else { 
usinitiloex (Garcalock)* 


// Create a binary semaphore for providing mutual exclusions so 
// that when the data record parameters are being set, the data 
// producer waits until the setting finishes. Refer to 
// getPacket() and setDataTypes() for more info. 
Lf (success C& 
((paramsema = usnewsema(arena, 1)) == NULL)) { 
success = FALSE; 
report_syserr(”creating a binary semaphore”, 
"FastrakClass: :initMultiprocessing”); 
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// Fork the parallel data producer as a lightweight thread which 
// shares the data space with its parent process that is the 
// consumer of the FASTRAK data. 


if (success) { 
Paneelleela = SproectpollGontinucusily, PR_SALL, (void *)this); 
hen (Daeoo Ll seid ==) =) jot 


success = FALSE; 
report_syserr(”spawning the producer process”, 


“FastrakClass::initMultiprocessing” ); 
} 


else { 
Signal (SIGCLD,SIG_IGN) ; 
#if DEBUG 
cerr << “Fastrak poll process spawned: pid = ” 
<< parpoll_pid << endl; 
#endif 


} 


a 
// 
ee 
es 
we 
// 
// 


} 
} 


jee77 if arena 


return (success); 
// end FastrakClass::initMultiprocessing() 


Function: getPacket 
Returns: 
Parameters: 
Summary: Read a packet from the FASTRAK. In single process mode, 
: it is called when the FASTTRACk user requests a data. In 
multiprocess mode, it is continuously called by the data 
Producer, poliContinucusly{), in a light weight process. 


void FastrakClass::getPacket() 


{ 


ine onus bur rer: 


(7 tiesto owing DiecCerert code 1S a critical section in multiprocess 
// mode. During a read operation of the FASTRAK, the data record 
// parameters cannot be changed, e.g, through setDataTypes() 
// which runs in parallel with this method. 
Te ei pawpelilipid Y= =1) { 

if (uspsema(paramsema) == -1) { // entering the critical section 

report_syserr(”getting semaphore”, 
“FastrakClass: :getPacket”); 


3 
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else { 
ji inecerial mode, ask for a packet from the FASTRAK. 
// Refer to pp 120, the 3 SPACE USER’S MANUAL. 
it Mwelecemport fd, 7P*, 1) 1s) { 
report_syserr(”sending FASTRAK P command’, 
"FastrakClass::getPacket”) ; 


/f/ SeBerSsersssssssasssssssssessssssasesssssssss=5 
// keep reading until device buffer is exhuasted 
do { 
unsigned int num_bytes_to_read = BUFFER_SIZE - read_index; 
int num_bytes_read = read(port_fd, 
&read_buffer[read_index], 
num_bytes_to_read); 


if (num_bytes_read > num_bytes_to_read) { 


Cerr <<) “Error: fstk fread Coo many bytes (aba ember). (7 
<< num_bytes_read << ”%,” << num_bytes_to_read 
<< f) Vines 
} 
1f (num_bytes_read == num_bytes_to_read) { 


full butter = TRUE, 
cerr << “Warning: fstk read max bytes: ” 
<< num_bytes_read << ”.\n”; 


// process the data read 

if (num_bytes_read > 0) { 
unsigned int index = 0; 
read_index += num_bytes_read; 


// while there is enough information for a packet from a 
// single station process the data: 


while (!kill_flag && ((read_index - index) > max_datarec_size)) { 


// make sure header info is the first few bytes 


if ((read_buffer[index] == 0x30) && 
( (read_buffer[index+1]&0xf£0) == 0x30) && 
(((int) (read_buffer[index+1]&0x0f) - 1) < 4)) { 
int station_num = (int) (read_buffer[index+1]&0x0f) - 1; 


Hes CP Tee ee PSF Te Oot Se TD TP Rig ee Coe St ON OT OT PT PP oat Cat ae Sed Cah Rat Tas 

// entering the critical section 

it (ussetlock(datalock) == —1) 
repe@et._syserr(”getting lock”, “packetizer’ ); 


memcpy (datarec_buf[station_num], &read_buffer[index], 


datarec_size[station_num] ); 
data_ready_flag = TRUE; 
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usunsetlock (datalock) ; {//, unlocking 
// exiting the critical section 


rae KKK KKK K EEK KKK KEKKKEKRKKKEK 


index += datarec_size[station_nun] ; 
} 
else { // find the header info and hopefully resynch. 
cerr << “Warning: resynching fstk ” 
<< “(index, read_index): = (” 


22 InG@eke2— 6 7 @-- seeaa index << “)\n"; 
while ((index < (read_index-1)) && 
!((read_buffer[{index] == 0x30) && 
((read_buffer[(index+1]&0xf0) == 0x30) && - 
(((int) (read_buffer[index+1]&0x0f) - 1) < 4))) { 
cerr << hex << (int) read_buffer[index]) << dec 
ok, aad ld 
index++; 


} 

cerr << hex << (int) read_buffer[{index]) << ” " 
<< (int) read_buffer[index+1] << dec 
es eee  neex-: ~ <= "index << endl; 


} 
} // while read_index-index 


// when done, shift the rest of the buffer down to the 
// beginning. 
if (index != read_index) { 
if (index > read_index) { 
€eane- 8 BEror: £Stk shifting too many bytes (” 
<< read_index << “-” << index << ”) .\n"’; 
} 
else { 
memcpy (read_buffer, &read_buffer[index], 
read_index-index) ; 
} 
} 
read_index -= index; 
ices = 0 
) // if num_bytes_read 


whee (hull igbpubier £& !kill flag); 
/7 exteing senenerrneiecal. sect ion 


if (parpoll_pid != -1) usvsema(paramsema) ; 
) // end FastrakClass::getPacket () 
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eee a nm i ee 
ed Function: sendCommand 

es Summary: Send a command to the FASTRACK. 

// Parameters: command string, its length, and initiating fen name 

Ea Returns: TRUE if the operation is successful. Otherwise, FALSE. 
eee ee a 8 eee 


int FastrakClass: :sendCommand(char* command, int length, char* source) 
{ 


me success = TRUE: 


ieource) ; 
#if DEBUG 
Cern << “command frome” << "Source << “:” 
<< command << endl; 
#endif 


// This is a critical section in multiprocessmmcede. 
if “(parpel ld pide) sa 
if (uspsema(paramsema) == -1) { // entering the critical section 
success = FALSE; 


report_syserr(”getting semaphore”, "“FastrakClass: :sendCommand’” ) ; 


if (write(port_fd, command, length) == -1) { 
success = FALSE; 
report_syserr(”sending FASTRACK command”, “FastrakClass: :sendCommand” ) ; 


data_ready_flag = FALSE; // See getPacket() for when it is set to TRUE. 


// exiting the critical section 
if (parpoll_pid != -1) usvsema(paramsema) ; 


#1f DEBUG 


@Gerr << “FASTRAK command sent: source being” << sources. 2m ; 
#endif 


return (success) ; 
} // FastrakClass: :sendCommand () 


Tf nn ne 8 Se == = 
f/f Function: convertData 

Lf Returns: 

// Parameters: data record ptr, number of elements, output vector 

lag Summary: Convert a DOS ordered bytes to Unix order (reverse) 

[mr rr rrr cer ce srrecnH 


void FastrakClass::convertData(char* data, int num_floats, 
float data_dest[]) 


Ghar *ptr = data; 
Char *Eptr = (Ghar *) data_dest; 
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) 


// 
// 


Hope 


} 


(int i=0; i<nwm_floaes; i++) { 
S(eptr+3) = “ptmry; 
SVEpCr+2). = Rt ri 
*(fptr+l) = *ptr++; 
= EpDer) = *ptr++; 
EUCE = 4 


// FastrakClass: :convertData () 


— oe em om om eww we ww ee i ee i i 


Function: convertl1é6éBITData 
Returns: 


// Parameters: data record ptr, number of elements, scale, output vector 


as 


Summary: Convert a Polhemus’s 16BIT format to IEEE floating point 


— meme ee ee ww we ew ww sw we we we we a eee i es 


Vorg Fast raxclass::convert!1cBiTData(char* data, int num_floats, 


} 


float scale, float data_dest[]}) 


Chara * Dtryeecata: 
char lobyte; 

char hibyte; 

Pile Slo@et lag, mum, 


for 


} 


(mee Oe aemume floats; 1++) { 
lobyte = *ptr++; 
hibyte = *ptr++; 
sign_flag = (int) hibyte&0x040; 
num = ((hibyte << 7) + (lobyte&0x7f))«cOx00l1fff; 
it (sign_flag) { 

num -= 0x02000; // 14 bit 2’s complement conversion. 

} 


edeameest(1] = scale*num; 


// FastrakClass: :convert1lé6éBITData() 
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i7 
ae 
oe 
ry 
Li 
ha 


Function: debugData 
Returns: 
Parameters: 
Summary: Write num_of_bytes starting from data_store as characters 
: to cerr. This is a convienience function used to examine 
the data packet read in from FASTRAK. 


void FastrakClass::debugData(char *data_store, int num_of_bytes) 


{ 


Gernr <2mvRecord length: “s<<) num of bytes << “\n7-; 
Sem <—5 i, 
fomwlittul = O;si < numeor bytes; 1+4+) 

cerr << Hex << (int) datasc@ore(i1)o<<adec << *|"; 
Serr <280 nn’; 


Function: detect Error 
Returns: 

Parameters: 
Summary : 


cm mm we ww ww ew we ae i ia ii 


imt FastrakClass::detectError() 


{ 


Ghar *Staiuon data: 
ime suceess = TRUE; 


// This method should only be used in critical sections. As a result, 
// no semaphore protection is needed here. 
for (int 1 = 0; 1 < FSTKaNUM STATIONS, 144+) 4% 

station_data = datarec[i]; 


switch (*station_data) { 
case ’0’: break; // No error for data record — do nmething 
case ‘2’: cerr << “Fastrak Type 2 Record received.\n’”; 
break; 

case ‘A’: cerr << BELL 
<< “HARDWARE ERROR found in Fastrak station” 
<< i+l - 
<< “ EPROM CHECK SUM. (character A)” << endl; 
break; 

Gaser C7 Cerr << BELL 
<< “HARDWARE ERROR found in Fastrak station” 
<< i+l 
<<,” RAM TEST. (chanaceern ¢) <= endl: 
break; 

Gases =) cerr <= BELL 
<< “HARDWARE ERROR found in Fastrak station” 
<< itl 
<< ” SELF-CALIBRATION. (character S$)” << endl; 
break; 


We 


FastrakClass.cc 


Gase 


Gase 


case 


case 


Case 


case 


case 


case 


Case 


oo bed ; 


ea 


te = 
: 


Cerr 


Cerr 


Cele 


Cer 


cer 


Geat: 


Cerner 


Cerr 


cerr 


22 


<< BELL 

<< “HARDWARE ERROR found in Fastrak station” 
<< i+l 

<< “ SOURCE/SENSOR ID PROM. (character U)” 

<< endl; 

break; 

<< BEE 

<< "SOFTWARE ERROR found in Fastrak station” 
<< i+1 

<< * CALCULATE TRACE OF S4TS4. (character a)’ 
=< ene. 

break; 

<< BELL 

<< “SOFTWARE ERROR found in Fastrak station” 
<< i+1 

“oe woolen DIVIDE. (character b)* << endl; 
break; 

<< BELL 

<< “SOFTWARE ERROR found in Fastrak station” 
<< i+1 

<< SELF-CAL A/D INPUT. (character c) ” 

<< endl; 

break; 

<< BELL 

<< “SOFTWARE ERROR found in Fastrak station” 
<< 1+1 

== SENSOR A/D INPUT. (character d)” << endl; 
break; 

<< BELL 

<< “SOFTWARE ERROR found in Fastrak station” 
<< 1+1 

<< “ OUT OF ENVELOPE. (character e)” << endl; 
break; 

<< BELL 

<< “SOFTWARE ERROR found in Fastrak station” 
<< i+1l 

<< * SELF-CAL OFFSET OVERFLOW. (character ff)” 
nee syale ile 

break; 

<< BELL 

<< “SOFTWARE ERROR found in Fastrak station” 
<< i+1 

wo RMD CALCULATION. (Character g)” << endl; 
break; 

<< BELL 

<< “SOFTWARE ERROR found in Fastrak station” 
<< i+1 

<<“) PATH steharacter im) “s<< endl; 

break; 
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} 


Gase ~ 17 scerr 


Case 7°] <-@cerr 


case ‘k’: cerr 


case ‘l’: cerr 


case ‘n’: cerr 


Gase 2p = -cCerr 


default : cerr 


} // end switch 
} // endetor 


return (success) ; 


// end detectError () 


23 


<< BELL 

<< “SOFTWARE ERROR found in Fastrak station” 
<< i+1 

<a Pande. (Character i)” << endie 

break; 

<< BELL 

<< “SOFTWARE ERROR found in Fastrak station” 
<< i+l 

<a) PARH3« (Character 3)” << endl: 

break; 

a ae =) an 

<< “SOFTWARE ERROR found in Fastrak station” 
<< i+l 

<< ”“ PATH4. (character k)” << endl; 

break; 

<< BELL 

<< “SOFTWARE ERROR found in Fastrak station” 
<< i+l 

<< ”“ SYSTEM RUNNING TOO SLOW. (character 1)” 
<_ end! ; 

break; 

<< BELL 

<< “SOFTWARE ERROR found in Fastrak station” << i+1l 
<< “ ATTITUDE MATRIX CALCULATION. (character n)” 
<< endl; 

break; 

<< BELL 

<< “SOFTWARE ERROR found in Fastrak station” 
<< i+1 

<< “ NORM OF XORVEC TOO LOW. (character p)” 
<< endl? 


break; 

<< BELL 

<< “UNKNOWN ERROR found in Fastrak station” 
<< i+] << ”: (character ” << “Sstatilonvadqca 


<a. o) RSet Code. 
m< Int (*station data) << endl; 
break; 
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a See essere See 
// Function: reportStateError 

as Returns: 

// Parameters: 

lef Summary: Report errors caused by trying to use inactive stations. 
ee ee ee ee ea 


void FastrakClass::reportStateError(char* location, 
FSTK_stations station_num) 


Gerr << BELL << “Error in using station” << station_num+l << ”’\n” 
2 ein *PastrakrGlvacs.ce*Prastrakclacs::” 
<< location << ”; inactive Station” << endl; 


nN 
// Function: checkReadError 

// Returns: TRUE if no read-data error, else FALSE 

// Parameters: station number 

Tf Summary: Check for read-data error. 

| [mmm tr rr rrr ccc ccce- 


int FastrakClass: :checkReadError(FSTK_stations station_num, 
char* source, FSTK_datatypes data_type) 


int success = TRUE; 


tion activye Sstatelistation num) ) { 
reportStateError(source, station_num) ; 
success = FALSE; 


iemecdaede De ctabre (Station num] [data_type!] < 0) { 
cerr << BELL << “Error in reading FASTRAK station” 
———=Statlon Oume) << ”.\n” 
<< ”“ in *FastrakClass.cc*FastrakClass::” << source 
<< “; unmrequested data type\n” << endl; 
success = FALSE; 


fees toneol ling t lag) { 
cerr << BELL << “Error in reading FASTRAK station” 
=o SGEaAClLOn Mim+l << ”.\n” 
<< “ in *FastrakClass.cc*FastrakClass::”% << source 
<< “; polling process suspended\n” << endl; 
success = FALSE; 


return (success) ; 
jue 7 BE aSterakcl] ace? :checkreaderror () 
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a i a nn 
ay Function: suspend 

// Returns: TRUE if the suspension is successful. Otherwise, FALSE. 
// Parameters: 

ai Summary: In multiprocess mode, suspend the execution of the 

ae : parallel polling process. 

Dm a ee ee ee 


int FastrakClass: :suspend() 


{ 
int success = TRUE; 


ico ieepohl  nGerlacg)) { 
iE (pareolep1G 1= NUR et 
if (uspsema(paramsema) == -1) { 
success = FALSE; 
= report_syserr (“suspending the polling process”, 
“RastrakClass: :suspend”); 


} 
else { 
is polling flag = FABSE; 
} 
} 
#if DEBUG 


cerr << “Parallel polling process suspended: “ << success << endl; 
#endif 
} 


return(success); 


a 
Li Function: resume 

aa Returns: 

// Parameters: 

i/ Summary: Resume the execution of the parallel polling process. 

ee ee Se SS ee ee 


void FastrakClass: : resume () 
{ 
ier (lic polling. flag) { 
if (parpoll_pid != NULL) usvsema (paramsema) ; 


ieapeliangetlbag = TRUE; 
#if DEBUG 
cerr << “Parallel polling process resumed. \n”%; 
#endif | 
} 
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ne ee ee ee ee SR eH Se SH 
as Function: setAlignment 

7 Returns: TRUE if the operation is successful else, FALSE. 

// Parameters: station number 

tes Summary: set the alignment of a station. 

(0 Fee SS SS SSS SS SS SR SS SSE a SS SS SS SS 8 


int FastrakClass::setAlignment(FSTK_stations station_num, 
COnsceetloal Origin(3], 
Sense getoae xs point (3), 
SOncteerloac Y point([3]) 


if (tactive_state[station_num]) { 
reportStateError(”“setAlignment”, station_num) ; 
return (FALSE) ; 

) 


// Construct the “A” command to set the alignment of the station. 

// Refer to pp 42-49, the 3 SPACE USER’s MANUAL for details. 

Static ehar commanma(100); 

See memrecmmemca) Acad, o.1f,%5-16,%.1£,%3.1£,%.1£,%3.1£,%.1£,%.1f£,%3.1f\r" 
station_num+l, 
SEiqiumgeik <|;, OriginieolkY], origin|FSTK_Z], 
MeDOUnewPolkKes |, xX -pOlnt [FSTK_Y], x.point([FSTK_2Z], 
Vo pDOincmesTK A), vV_poimt([FSTK_Y], y point [FSTK_Z]); 


int success = sendCommand(command, strlen(command), 
"PastrakClass::setAlignment”); 


1f (success == TRUE) { 
Fou. (int 1 = 0; i < 3: i++) 


{ 


CEL t | 
Xeoime [1 | 
vVepolne (21; 


elmlemnment (Station num] [0] [1] 
aleummene | Station num) {1] [1] 
alignment[station_num] [2] [i] 


#if DEBUG 
cerr << “FASTRAK alignment set.\n”; 
#endif 


SELUrN (Success) ; 
} // FastrakClass::setAlignment () 
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YL nn ne = = 
ae Function: resetAlignment 

77 Returns: TRUE if the operation is successful else, FALSE. 

// Parameters: station number 

fl. Summary: Reset the alignment of a station. 
ee ee a 


int FastrakClass::resetAlignment(FSTK_stations station_num) 
{ 
if (!active_state[station_num]) { 
reportStateError(”resetAlignment”, station_num) ; 
return (FALSE) ; 
} 


// Construct the ”“R” command to reset the alignment of the station to 
// default. Refer to pp 50, the 3 SPACE USER’s MANUAL for details. 


char command[10]; 
sprintf(command, “R%d\r”, station_num+1) ; 


int success = sendCommand(command, strlen(command), 
“FPFastrakClass::resetAlignment”) ; 


ii weeicecess == TRUE { 
for (int i = 0; i < 3; i++) 
for (int j = 0; j < 3; jtt) 
alignment (station_num][i][j] = 0.0; 


alignment [station_num] [1] [0] 
alignment (station_num] [2] [1] 


Oe // X% Girveceren 
Dede (i Y Aimeerien 


#if DEBUG 
cerr << “FASTRAK alignment reset.\n’; 
#endif 


return(success) ; 
} // FastrakClass::resetAlignment 
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// Function: 
rae Returns: 
// Parameters: 
Ty Summary: 


void FastrakClass:: 


fey Tova (lhl te 


Gierenia (1) 


getAlignment 


Cragin, 


X-axis, 


Station number 
Get the alignment of the station. 


x perme (i) 
Y_poeine [1] 


} 


Ve of) Bast raxc lass 


// Foner Ton: 
Ley, Returns: 
// Parameters: 
// Summary : 


|| | a 


alec <2 5 ae 


setBoresight 
TRUE if the operation is successful else FALSE. 
station number and orientation angles 

Set the boresight of a station. 


and y-axis vectors 


{ 


getAlignment (FSTK_stations station_num, 


EloOat Origin (3 |, 


PiGatexapOdmtels ly float y point [3] ) 


alignment [station_num] [0} [i]; 
alignment [station_num) [1] [i]; 
alignment [station_num] [2) [i]; 


::getAlignment() 


int FastrakClass::setBoresight(FSTK_stations station_num, 
const float orient[3]} 


Peeieceivesstace| station num)) { 


reportStateError(”setBoresight”, 


return(FALSE) ; 


station_num) ; 


// Construct the ”G” command to establish the boresight reference 


// angles. 


// acetals. 


Rerer ce pp S51 — 54, 


char command[50]; 


sprintf (command, 
station_num+l, 


Int success 


aie 


// Construct the 
L{eROLCrEVEO: Ppt oo, 
sprintf (command, 


sendCommand (command, 


the 3 SPACE USER’s MANUAL for 


weet lies. lt,s.1f\r” 
Srelent(PSTK AZ]; 


orient [FSTK_EL]), orient[FSTK_RO] ); 


strlen(command), 


"“FastrakClass::setBoresight”); 


(Success == TRUE) { 
boresight [station_num) [FSTK_AZ] = 
boresight [station_num) [FSTK_EL] 
boresight [station_num] [FSTK_RO] 


13 “ur 


“Bea\r” ' 


command to 
the 3 SPACE USER’s MANUAL for details. 


orient [FSTK_AZ)] ; 
orient ([FSTK_EL]; 
orient [FSTK_RO] ; 


set the line_of_sight of the station. 


station_num+1); 
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success = sendCommand(command, strlen(command), 
“FastrakClass::setBoresight”); 


#if£ DEBUG 
cerr << “FASTRAK boresight set.\n’; 
#endif 


return(success); 
} // FastrakClass::setBoresight () 


0 SS SSS SS 2 SSS = SSS SS Sa SS 
ff Function: resetBoresight 

OE Returns: TRUE if operation is successful 

// Parameters: station number 

Tf Summary: Reset the boresight. 

ee a ee ee 


int FastrakClass::resetBoresight (FSTK_stations station_num) 
{ 
TPC racktive _state[Station numm { 
reportStateError(”resetBoresight”, station_num) ; 


return (FALSE) ; 


// Construct the ”“b” command to reset the boresight of the station to 
// default. 

// Refer to pp 55, the 3 SPACE USER’s MANUAL for details. 

char command (10); 

sprintf(command, “b%d\r”, station_num+1) ; 


int si@eess = 
sendCommand(command, strlen(command), “FastrakClass::resetBoresight”) ; 


ME (Success == TRUE) { 
boresight [station_num] [FSTK_AZ] 
boresight [station_num] [FSTK_EL] 
boresight [station_num] [FSTK_RO} = 


{I 
Qo ©C 


(2) (OS) (S& 
=e “=e 


=e 


#if DEBUG 
cerr << “FASTRAK boresight reset.\n”; 
#endif 


return (success) ; 
} // FastrakClass::resetBoresight() 
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a 
od 


ee ee ee ee tt el 


Function: getBoresight 
Returns: euler angles defining the boresight 


// Parameters: station number 


es 


Summary : 


-— = oe oe oe om om oe om ao oe ae iw oe oe om om om a om om om om om 6 we om om om em me om om om o ae a ec am = @ oe oe ae = a om am Ss ic ee ce ae ae ee oe oe ce oe om oe oe ae ice oe ee oe = 


void FastrakClass::getBoresight(FSTK_stations station_num, float orient[3}) 


{ 


orlent [FSTK_AZ]} boresight[station_num]} [FSTK_AZ]}; 
orient [FSTK_EL]} boresight[station_num]} [FSTK_EL}; 
orient [FSTK_RO} boresight [station_num]} [FSTK_RO} ; 


// FastrakClass: :getBoresight() 


it 


Function: setHemisphere 

Returns: TRUE if the operation is successful. Otherwise, FALSE. 
Parameters: station number and new zenith vector 

Summary: Set the hemisphere of a station. 


ee ee ee ee ee ee ee ee ee ee ee ee 


int FastrakClass::setHemisphere(FSTK_stations station_num, 


( 


GOnSe- tloat Zenienh(3)) 


if (tactive_state[station_num}) { 
reportStateError(”setHemisphere”, station_num) ; 
return (FALSE) ; 

) 


// Construct the ”"H” command to set the hemisphere of the station. 
// Refer to pp 88 - 92, the 3 SPACE USER’s MANUAL for details. 
char command[50]; 
Summer toonmmana, “Hsa,%.1f£,%.1f,%.1f\r", 
station_num+1l, zenith[FSTK_X}, zenith[FSTK_Y}], zenith[FSTK_Z]}); 


int success = 
sendCommand(command, strlen(command), “FastrakClass::setHemisphere’” ) ; 


Lie Sucece sou = TRUE) ( 
hemisphere[station_num]} [FSTK_X] 
hemisphere[station_num} [FSTK_Y} 
hemisphere[station_num]} [FSTK_Z} 


zenith [FSTK_X]; 
ZONIEM PES TKY]; 
zenith [FSTK_Z} ; 


#if DEBUG 


cerr << “FASTRAK hemisphere set:” 
— Zen vetieolh x), ao pe << Zenith([FPSTK_Y} << ", * 
Ronen venteotkes) <<) “~.\n" ; 


#endif 


} 


return (success); 
// FastrakClass: :setHemisphere () 
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1 i Neek eat eat iecteateatoeteatententeententartantenteaientaniententeientastententadestententertenertanadedetenatetertenedentatetertetteatetetetetetetetatentatetetetetetetete 
as Function: resetHemisphere 

// Returns: FALSE if station inactive or reset fails, TRUE otherwise 
// Parameters: station number 

Wa Summary: Reset the hemisphere. 

= = ee ee - Se ee ee a a = = = -- 


int FastrakClass: :resetHemisphere(FSTK_stations station _num) 
{ 


if (active stetelistationmmuim)) { 
reportStateError(”resetHemisphere”, station_num) ; 


return(FALSE) ; 
// Refer to pp 88 - 92, 3 SPACE USER’s MANUAL for the default. 
float default_zenith[3]; 
default_zenith[FSTK_X] 


Gefault_zenith[FSTK_Y] 
Gefault_zenith[FSTK_Z] 


| 
@) CS) f= 


° =e 


=e 


#if DEBUG 
cerr << “FASTRAK hemisphere reset.\n’”; 
#endif 


return (setHemisphere(station_num, default_zenith) ); 
} // FastrakClass: :resetHemisphere() 


ee eee 
ng Function: getHemisphere 

i? Returns: 

// Parameters: station number and zenith vector 

| Summary: Get the hemisphere of the station. 

SE ee ee 


void FastrakClass::getHemisphere(FSTK_stations station_num, float zenith[3]) 
{ 

zenith[FSTK_X] hemisphere[station_num] [FSTK_X] ; 

zenith[FSTK_Y] hemisphere[station_num] [FSTK_Y]; 

zenith [FSTK_Z] hemisphere[station_num] [FSTK_Z]; 
} // FastrakClass: :getHemisphere() 
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Vf mm rr er rrr rar r rr eran aa 
es Function: setUnits 

// Returns: TRUE if the operation is successful. Otherwais FALSE. 

// Parameters: 

// Summary: Set the position measuring unit for the FASTRAK. 
Ce ee 


int FastrakClass::setUnits(FSTK_units pos_units) 
{ 
// Construct the “U/u” command to set the unit for the FASTTRAC data. 
// Refer to pp 122 - 124, the 3 SPACE USER’s MANUAL for details. 
char command [2]; 
tee pesslites == FSTK CENTIMETER) 
Sereey (command, “u\0”); 
else if (pos_units == FSTK_INCH) 
Sepepy (commana, ~U\0*) ; 
else { 
Come ECrOr: lnivalid units specification * << pos_units << endl; 
return (FALSE); 


int success = sendCommand(command, 1, "FastrakClass::setUnits”); 
if (success) units = pos_units; 


#i1f£ DEBUG 
cerr << “FASTRAK position units set.\n”; 
#endif 


return (success) ; 
ny / senmegeracstrakClass::setUnits 


ee i oe 
// Function: setDataTypes 

as Summary: Specify the requested data types for the station. Before 
La : data can be read from the FASTRAK, the types of the data 
at : needed must be specified. By default, position 

Ves : coordinates and Euler orientations are returned from each 
Ws = ctac lon 

// Parameters: station number, datatype mask 

7 Returns: TRUE if the operation is successful. Otherwise, FALSE. 
By 


int FastrakClass: :setDataTypes(FSTK_stations station_num, short mask) 
{ 
if (tactive_state{station_num]) { 
reportStateError(”setDataTypes”, station_num) ; 


return(FALSE); 


int success = TRUE; 


77 The Etollowing piece Of Code is a critical section in multiprocess 
// mode. When data record parameters are being updated, data records 
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// cannot be allowed to be read by getPacket() which runs in 
// parallel with this method. 


if ((parpoll_pid != -1) && (!param_set_flag)) { 
// entering the Critical section 
if (uspsema(paramsema) == -1) { 


success = FALSE; 
report_syserr(”getting semaphore”, 
“FastrakClass::setDataTypes” ); 


// Adjust the data record parameters. 


Tig ees 
fstk_packet_size -= datarec_size[station_num]; 
datarec_size[station_num] = FSTK_HEADER_SIZE; 


for (1 =—80; i = FSTK NUM DATATYPES i) 
datatype_start[station_num] [i] = -1; 


7// Construct the “©” Gommana to specify thew rype of data 
// we want from the FASTRAK station. 

// Refer to pp 97 - 111, the 3 SPACE USER’s MANUAL for details. 
char command[20]; 

sprintf(command, “0%d”, station_num+1) ; 


if (mask & FSTK_COORD_MASK) { 
Streat (Command, *,27); 
datatype_start([station_num] [FSTK_COORD_TYPE] 
datarec_size[station_num] ; 
datarec_size[station_num] += FSTK_COORD_SIZE; 


if (mask & FSTK_EULER_MASK) § ({ 
SEreat (command, ~,4"); 
datatype _start[station_num] [FSTK_EULER_TYPE] 
datarec_size[station_num]; 
datarec_size[station_num] += FSTK_EULER_SIZE; 


Hl 


if (mask & FSTK_XCOS_MASK) { 
SErcat (command, ”75”%); 
datatype_start([station_num] [FSTK_XCOS_TYPE] = 
datarec_size[station_num]; 
datarec_size[station_num] += FSTK_XCOS_SIZE; 
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(mask & FSTK_YCOS_MASK) { 


Genecat(conmand, = ,6 ); 
datatype_start[station_num] [FSTK_YCOS_TYPE] = 


datarec_size[station_num]; 
Gatarec_size[station_num] += FSTK_YCOS_SIZE; 


ie 


(mask & FSTK_ZCOS_MASK) { 


line 
Serest (command, “~,/7"); 
datatype_start[station_num] [FSTK_ZCOS_TYPE] = 
datarec_size[station_num]; 
datarec_size[station_num] += FSTK_ZCOS_SIZE; 


(mask & FSTK_QUAT_MASK) { 


Sepeac(commancad, *,11"); 
datatype_start[station_num] [FSTK_QUAT_TYPE] = 


Nipe 
datarec_size[station_num] ; 
datarec_size[station_num] += FSTK_QUAT_SIZE; 


} 


(mask & FSTK_16BIT_COORD_MASK) { 


ule 
Sepeatk (command, “~,16"); 
datatype_start[station_num] [FSTK_16BIT_COORD_TYPE] = 
datarec_size[station_num] ; 
datarec_size[station_num] += FSTK_16BIT_COORD_ SIZE; 
} 
if (mask & FSTK_16BIT_EULER_MASK) { 
Sereae (command, ”,197); 
datatype_start [station_num] [FSTK_16BIT_EULER_TYPE] = 
datarec_size[station_num] ; 
datarec_size[station_num] += FSTK_16BIT_EULER_SIZE; 
) 
if (mask & FSTK_16BIT_QUAT_MASK) { 


Stmcat (eonmana, —7) 20”); 
datatype_start[station_num] [FSTK_16BIT_QUAT_TYPE] = 


datarec_size[station_num]; 
datarec_size[station_num] += FSTK_16BIT_QUAT_SIZE; 


(mask & FSTK_CRLF_MASK) { 


Siheat (commana... ° 1% } > 
datarec_size[station_num] 


ae 
+= FSTK_CRLFE_SIZBE; 


SEnecae conmanad, ~\r”); 
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// recompute the maximum station data record size 
Max_datarec_size = 0; 
for (i1=0; i<FSTK_NUM_STATIONS; i++) { 
if (datarec_size(station_num) > max_datarec_size) 
max_datarec_size = datarec_size([(station_nun]; 


#i£ DEBUG 
cerr << ”“** setDataTypes:\n” 
a rme Command: “~ << command << “\n" 
<< Expecting station” << station_num+1 


<< " Ee@econtaim ” <= datarec size (stations) 
<<_ Bey bes << endl: 
#endif 


// Note that sendCommand() is not be used here because 
// adjusting data record params and sending the command must be in 
// the same critical section. 
if (write(port_fd, command, strlen(command)) == -1) { 
success = FALSE; 
report_syserr(”sending FASTRAK O command”, 
“PastrakClass::setDataTypes”) ; 


datatype_mask[station_num]) = mask; 
fstk_packet_size += datarec_size(station_num] ; 


for (i = station_num+1; i < FSTK_NUM_STATIONS; i++) { 
if (active_state[i]}) { 


for (j = 0; j < FSTK_NUM_DATATYPES; j++) 
datatype_start([(i}]{j] += datarec_size[{station_num] ; 


} 


data_ready_flag = FALSE; // See getPacket() for when it is set to TRUE. 


if ((parpoll_pid != -1) && (param_set_flag == FALSE) ) 
usvsema (paramsema) ; // exiting the critical section 
#if DEBUG 


cerr << “FASTRAK data type specified.\n”; 
eerr << "Packet size setetro “~ << Stk packet _ size << “ bytes. (nn: 
#endif 


return (success) ; 
} // end FastrakClass::setDataTypes 
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Li 
es 
‘7 
as 
if 
ich 
ee 
(ey 
i 
// 


ee ee ee ee ee eee eee eee eee eee ee ew ee eS ew ew ew ew ee ea em eae eal ae ee ee 2 ee 2 


Function: setState 


Summary: Set the sate of the station: TRUE (active) or FALSE 
; (inactive). Note that this routine may seem to be more 
complex than necessary. The complexity is due to the need 
to handle the different requirements at and after the 
initialization stage. This routine can be simplified by 
spliting it into two separate ones. However, then the 
program becomes longer. 


Parameters: station number, state 


Returns: TRUE if the operation is successful. Otherwise, FALSE. 


int FastrakClass::setState(FSTK_stations station_num, int active_flag) 


{ 


ite 


('active_setting(station_num]) { 
reportStateError(”setState”, station_num) ; 
return (FALSE) ; 


tiewesuccess = TRUE; 


ee 
as 
La 
// 
ae 


When fstk_packet_size = 0, the program is in the initialization stage, 
where setState() is called from checkState(). Otherwise, 
fstk_packet_size > 0. The initialization has finished and setState() 
is called by the FASTRAK user. | 


(MTSE packer size == 0) | | 
(active_state[station_num] != active_flag)) { 
Titel jack ive Seation = -ljogmumeactives = 0; 


for (1 = 0; i < FSTK_NUM_STATIONS; i++) { 
if active stare (i) ) 4 
num_actives++; 
active_station = i; 


// Error! trying to deactivate the last remaining station. 
// At any time, at least one station must be active: 
if ((mum_actives <= 1) && 
(eee) Jemstaulomv== Station=num) && !(active_flag)) { 
cerr << BELL << "Error in setting FASTRAK station” 
<< station_num+l << ” state.\n’” 
<< ” in *FastrakClass.cc*FastrakClass: :setState” 
<< ” At least, one station must be active at any time.” 
<< endl; 
return (FALSE); 


// Construct the “1” command to set the state of the station. 

// Refer to pp 128 - 131, the 3 SPACE USER’s MANUAL for details. 
char command([(10]; 

mt State cma = 0; 

if (active_flag) state_cmd = 1; 
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Ssprinti(eemmand, “lsd,@d\r”, station_num+1, state_cmd); 


#if DEBUG 
Cerr << *** setState: \n” 
ne ed The command: ” << command << endl; 
#endif 
// Mise tose rie Gal Section. 
Ve A parpolimeaad '= —1)> { 
if (uspsema(paramsema) == -1) { // entering the critical section 
success = FALSE; 
report_syserr(”getting semaphore”, “FastrakClass::setState’”)}); 
} 
} 
// Note that sendCommand(} is not be used here because 
// sending the command and updating data record params must be in 
// the same critical section. 
if (write(port_fd, command, strlen(command)) != -1) { 
active_state[station_num] = active_flag; 
// Update data record parameters when the change is from ACTIVE 
// to INACTIVE. On the other hand, when the change is from 
// INACTIVE to ACTIVE, setDataTypes() is called to update 
// record parameters. 
if (!active_flag && (fstk_packet_size > 0)) { 
fstk_packet_size -= datarec_size[station_num] ; 
for (i = station_num+i; i < FSTK_NUM_STATIONS; i++) { 
if (active_state[i])} { 
for (j = 0; j < FSTK_NUM_DATATYPES; j++) 
datatype_start[i][{j] -= datarec_size[{station_num] ; 
) 
} 
Gabarec s1zZe(Sstation nmum| = 0; 
for (i = 0; i < FSTK_NUM_DATATYPES; i++) { 
Gatatype start [stationanumii{1)] = -1; 
} 
} 
else if (active_flag) { - 
param_set_flag = TRUE; 
setDataTypes(station_num, FSTK_DEFAULT_MASK) ; 
param_set_flag = FALSE; 
} 
} 
else { 


} 


Success = FALSE; 

report_syserr(”sending FASTRAK 1 command”, 
"FastrakClass::setState” ); 

// end if (write()} 


data_ready_flag = FALSE; // See getPacket() for when it is set to TRUE. 
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//-exiting the critical section 
if (parpoll_pid '!= -1) usvsema(paramsema) ; 
) 


#i1£ DEBUG 
ceerr << “FASTRAK station” << station_num << ” state set.” << endl; 
#endif. 


return (success) ; 
) // end FastrakClass::setState() 


a eee 
is Punction: copyBuffer () 

77 Summa ry : 

// Parameters: 

Lae Returns: 
a a a a a == - === 


void FastrakClass::copyBuffer () 
{ 
// Use uspsema(paramsema) instead of ussetlock(datalock) if the 
// object of this class is to be used in multiple processes. 
while (!data_ready_flag) ; 
if (ussetlock(datalock) == -1) // locking for update 
report_syserr(”getting lock”, ”“FastrakClass::readData”); 


for (int station_num=0; station_num<FSTK_NUM_STATIONS; station_num++) 
memcpy (datarec({station_num], datarec_buf[station_num], 
datarec_size(station_num]); 


// Use usvsema(paramsema) if the object is to be used in multiple 
// processes. 


usunsetlock (datalock) ; Un Oe ng 
} 
Gn i a a nn en a nn we ee 
gs Function: readData 
7 Summary: Decompose the data packet in the current data buffer. For 
1 >: a succesful read, data_dest[(] contains the required type 
ie : Of data from the specified station. Note that (1) 
fa, : Gata_dest[] must be a 4-element array for quaternions; 
// : for the other types, it is a 3-element array; (2) old 
ee : Gata can be reused if invalid data packets were read by 
Le : getPacket(). 
// Parameters: station number, required data types 
// Returns: TRUE if the read is succesful. Otherwise, return FALSE. 
[ [----------- o-oo - - - - - -  - -  - -  - -  -  -  -  - -- 


int FastrakClass: :readData(FSTK_stations station_nun, 
FSTK_datatypes data_type, 
float data_dest[]) 
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if (checkReadError(station_num, “readData”, data_type) == FALSE) { 
return( FALSE) ; 


Vx 
// Use uspsema(paramsema) instead of ussetlock(datalock) if the 
// object of this class is to be used in multiple processes. 
while (!data_ready_flag) ; 
if (ussetlock(datalock) == -1) // Veecking tor vedere 
report_syserr(”getting lock”, “FastrakClass::readData”); 
ae f 
#if DEBUG 
Gerr << "** reqdbata: \n” 
aa” Shaklon =< Scrat lon num. << ° \n” 
<a record Start = ~*~ << datarec([station num! 
<< "; record size = ” << datarec_size([station_num] 
<< "; total size = ” << fstk_packet_size 


<<’ smddiea CYpe ctarte= ” 
<< datatype_start[station_num] [data_type] << endl; 


debugData(datarec{station_num], datarec_size[station_num]); 
#endif 


// the starting position of the type of data wanted in the buffer 
char* start_pos = datarec[station_num] + 
datatype_start[station_num)] [(data_type] ; 


switch (data_type) { 

case FSTK_COORD_TYPE: 

case FSTK_EULER_TYPE: 

case FSTK_XCOS_TYPE: 

case FSTK_YCOS_TYPE: 

case FSTK_ZCOS_TYPE: 
convertData(start_pos, 3, data_dest); 
break; 


Case FSTK_OQUAT TYPE: 
convertData(start_pos, 4, data_dest) ; 
break; 


case FSTK_16B0TSé00RD TYPE: 
im WMnitse == FSTK CENTIMETER) 
convert1l6BITData(start_pos, 3, 
FSTK_16BIT200.CM, datamdest); 
else 
convert1lé6éBiITData(start_pos, 3, 
FSTK_16BIT_TO_INCHES, data_dest) ; 
break; 


case FSTK_16B1T EULER_TYPE: 
convertl6éBITData(start_pos, 3, 
FSTK_16BIT_TO_DEGREES, data_dest) ; 
break; 


es 


FastrakClass.cc 40 


case FSTK_16BIT_QUAT_TYPE: 
converti6BiTData(start_pos, 3, 
FSTK_16BIT_TO_QUAT, data_dest) ; 
break; 


(hes 
// Use usvsema(paramsema) if the object is to be used in multiple 
// processes. 
usunsetlock (datalock); 7 fee hg 
wi 
PerurEn (TRUE); 
} // end readData() 


[ [mmr rr rr cr ccc ccce 
es Function: getHmatrix 

i Summary: Read a homogeneous transformation matrix. On a successful 
ee (recite memuipper lett 3x3 submatrix of matrix([] [] 

aes : contains a tranformation matrix constructed from the 

a ; X-cosin, Y-cosin and Z-cosin vectors of the station with 
ff : X-consin in the first row, Y-cosin in the second, and 

// : Z-cosin in the third; if FSTK_COORD_TYPE has been chosen 
if >: in setDataTypes, the fourth COLUMN will contain the 

i7 : position of the sensor wrt the transmitter, otherwise, 

ii ieetcmmhlec wiehm O. The fourth ROW is filled with 0’s 
// ~exveepere hat, Macrix[3)[3]-= 1. 

// Parameters: station number 

If Returns: TRUE if the read is succesful. Otherwise, return FALSE. 
niin ee a ee 


int FastrakClass: :getHMatrix(FSTK_stations station_num, 
Pieadtematrix(4|]([4)) 


// the starting pos. of the types of data wanted in the data record 
Giiare* Start_pos; 


Mae cise 1. O | 
Mekiioe( 3) [3] 
/* 


Madea oo kn eae InatcGi xis) (2) = 0; 
Ales 


// Use uspsema(paramsema) instead of ussetlock(datalock) if the 
// object of this class is to be used in multiple processes. 
while (!data_ready_flag) ; 
if (ussetlock(datalock) == -1) // locking for update 
report_syserr(”getting lock”, ”“FastrakClass::getHmatrix”) ; 
ay 


// get the position vector if FSTK_COORD_TYPE has been specified 
if (datatype_mask[station_num] &FSTK_16BIT_COORD_ MASK) { 
float pos([3]; 
Start_pos = datarec[station_num] + 
datatype_start [station_num] [FSTK_16BIT_COORD_TYPE]; 
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if {(Wnhits == FSTRACENDIMETER) 

COlvVere ob bivbataistart pes,ms, FSTK_#6BIiT sfOeCMieepos) ; 
else 

convertl16BITData(start_pos, 3, FSTK_16BIT_TO_INCHES, pos); 


for (int 1=0; 1<3; 14+ )jmaesiboie ) (3 }eeepos[i le 
} 
else if (datatype_mask[station_num]&FSTK_COORD_MASK) { 
float spos|3); 
start_pos = datarec[station_num) + 
datatype_start[station_num] [FSTK_COORD_TYPE] ; 
convertData(start_pos, 3, pos); 


ECu wie —O; ea) 2++) Matrix([1] [3] = pos([i); 
} 
else { 

matrix[(0](3] = matrae[1}][3] = mattrix[2] [3] ="@% 


} 


// get the rotation matrix one of three ways 
if (datatype_mask[station_num]) & (FSTK_QUAT_MASK|FSTK_16BIT_QUAT_MASK)) { 
Eloat quat [4]; 


if (datatype_mask [station_num] &FSTK_16BIT_QUAT_MASK) { 
Start_pos = datarec[station_num] + 
datatype_start[station_num]} [FSTK_16BIT_QUAT_TYPE] ; 
convertl16BITData(start_pos, 4, FSTK_16BIf TO.QUAT auat); 
} 
else { 
start_pos = datarec[station_num] + 
datatype_start[station_num] [FSTK_QUAT_TYPE] ; 
convertData(start_pos, 4, quat); 
} 
las HSUMsece locktdatalock) > ff UM ee hing 


// compute the rotation matrix from the quaternion info 
figatesoe— gua [i |/=quat (1); 
bliGgabe yy — sua (2) *quat (2); 
float zz = quat(3)*quat [3]; 
tloat xye="quat (1) *quat(2])>; 
PlOat Vz = cuat (2 quar |.) > 
ElOata 2 = eee | I) *quat [3]; 
float sx = quat[0]*quat([1]; 
PiOteacye=eguat | Oo) “Guat [2]; 
ELOgtaSZ.—seuac! Oo] ~guat [3]; 


Mat ruetomne | = 1.0 = 2.0* (yy + Zz); 
Waerseearc| il) =] 220" (xy = sz); 
Maem) o= 2o05tKz + SV); 
Meese (iol = 220% (xy + Sz); 
Tae Gait) = 1.0 = 2.0* (xx + ZZ); 
Maer ices eae O* (YZ — Sx); 
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BO (O07 ee Se iss 

2. 0* (Vze4- sx); 

1.0 = 2.0* (xx + yy); 


mMaerix(2)(0] 
matrix(2) (1) 
Metrix (2) [2] 


) 
else if (datatype_mask[station_num]& 
(FSTK_EULER_MASK|FSTK_16BIT_EULER_MASK)) { 
float angles([3]; 


if (datatype_mask[station_num]&FSTK_16BIT_EULER_MASK) { 
Start_pos = datarec[station_num] + 
datatype_start[station_num] ([FSTK_16BIT_EULER_TYPE] ; 
convert1lé6éBITData(start_pos, 3, FSTK_16BIT_TO_DEGREES, angles); 
) 
else { 
Start_pos = datarec[station_num] + 
datatype_start([station_num] [FSTK_EULER_TYPE] ; 
convertData(start_pos, 3, angles); 


// usunsetlock (datalock) ; ie unloeking 


// compute rotation matrix from the euler angle info 
angles[FSTK_AZ] *= DTOR; 
angles[FSTK_EL] *= DTOR; 
angles (FPS Tk2b0 + =—BIOR; 


float ca = cos (ame@les (FSTK_AZ]); 
float sa = sin(angles[FSTK_AZ]); 
float ce = cos(angles[FSTK_EL]); 
float se = sin(angles[FSTK_EL]); 
float cr = cos(angles[FSTK_RO]); 
Gl@aeect = Sin(angles (FSTK_RO]); 


float sesr se*sr; 
tleatececr = Se*cr: 


matrix[(0) [0] = ca*ce; 
matrix[(0])[1] = ca*sesr - sa*cr; 
matrix[0](2]) = ca*secr + sa*sr; 
Mae tix) | [0] = sa*ce; 
matrix{1])[1]) = sa*sesr + ca*cr; 
Matrix{(1)([2) = sa*secr - ca*sr; 
Matrix[2] [0] = -se; 
matrix[(2)[(1]) = ce*sr; 
Mech io<i 2 ie pew ect cr, 
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else if ((datatype_mask[station_num] &FSTK_XCOS_MASK) && 
(datatype_mask[station_num] &FSTK_YCOS_MASK) && 
(datatype_mask[station_num] &FSTK_ZCOS_MASK)) { 
for (int row = 0; row <3; row++) { 
Start_pos = datarec[station_num] + 
datatype_start[station_num] [row + FSTK_XCOS_TYPE] ; 
convertData(start_pos, 3, matrix[row]); 


// usunsetlock (datalock) ; /7 Unlock img 
} 
else { 
Genre <<) BerOn: no Oflientaeion infomation to, build H=meaeneix\n? - 
Vas. usunsetlock (datalock) ; // un@eecking 


return (FALSE) ; 


return (TRUE): 
} // end getHmatrix() 


Tn 8 
1 Function: getPosOrient 

// Summary: Read the current position and orientation of the station 
ye] >; together. On a Ssuccessiul return, posit|)] containe ene 
ae : position and orient[] contains the orientation. The type 
ee : of the orientation, euler-angle and quaternion, is 

Us : determined by orient_type. Note that if orient_type is 
ii : FSTK_EULER_TYPE, orient is a 3-element array. Otherwise, it 
ya : Must be a 4-element array. 

-// Parameters: station number, type of orientation 

ey Returns: TRUE if the read is succesful. Otherwise, return FAUSE. 
ff rr = === 


int FastrakClass::getPosOrient(FSTK_stations station_num, 
FSTK_datatypes orient_type, 
Eloat pos[3], fLlcat crienel| 


Chiar*® Start pos; 


if (checkReadError(station_num, “getPosOrient”, 
Orient_type) == FALSE) { 
return (FALSE); z 


/* 
// Use uspsema(paramsema) instead of ussetlock(datalock) if the 
// object of this class is to be used in multiple processes. 
while (!data_ready_flag) ; 
it (Uuesepleock(datalock) == -1) // locking for update 
bepertosyserr(*getting lock”, *“FastrakClass::getPosOmien: ); 
sf 
// get the position vector 
if (datatype_mask[station_num] &FSTK_16BIT_COORD_MASK) { 
start_pos = datarec[{station_num] + 
daeaeype stant (|Station_num)] [FSTK_16B1T _ COORD_TYPE)]; 
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ifeiunits == FSTK_CENMeMETER 
Convert l6BilTData(starespos, 3, FSTK_16BIT_TO_CM, pos); 
else 
convertl16BIiTData(start_pos, 3, FSTK_16BIT_TO_INCHES, pos); 
) 
else if (datatype_mask[station_num]&FSTK_COORD_MASK) { 
Start_pos = datarec[station_num] + 
datatype_start[station_num]} [FSTK_COORD_TYPE] ; 
convertData(start_pos, 3, pos); 
) 
else { 
Gerr=< “Errors: no position type selected in ” 
<< “FastrackClass: :getPosOrient\n’; 
return(FALSE) ; 


// get orientation vector 
Start_pos = datarec[station_num] + 
datatype_start[station_num] [orient_type] ; 


Switch (orient_type) { 
case FSTK_EULER_TYPE: 
convertData(start_pos, 3, orient); 
break; 


case FSTK_QUAT_TYPE: 
convertData(start_pos, 4, orient); 
break; 


case FSTK_16BIT_EULER_TYPE: 
convertlé6BITData(start_pos, 3, 
FSTK_16BIT_TO_DEGREES, orient); 
break; 


case FSTK_16BIT_QUAT_TYPE: 
convertlé6éBIiTData(start_pos, 3, 
Pork 16BiT_TO_QUAT, orient); 
break; 
default: 
@errn <<.)"Error: invalid orientation type specified in ” 
<< “FastrackClass::getPosOrient\n"”; 
return (FALSE) ; 


yee 

usunsetlock (datalock) ; yam locking 
a | 

SeLUrh TRUE): 
) // end getPosOrient () 
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fastrak.dat 1 

+ KKK KKK KKK KK KEK KH KKK KK KEK KEK KEK KK KK KEK KKK KEK KKK KKK KEK KKK KKK 

# FILENAME: fastrak.dat 

# PURPOSE: configuration file for Body class using 

# four fastrak sensors 

# AUTHOR: P F Skopowski 

+ Pere: 1 Jul 96 

# COMMENTS : 

# The file format: 

# a). A line starting with a ’#’ is a comment line. 

# bi. Baca line must mot contain more than 255 characters. 

# c). Maintain the order of the parameters (i.e., the station 
# parameters, hemisphere and alignment, must be the last 
# Dare of Ehe tile) . 

# KR KKK KEK KKK KKK EK KEKE KEK KKK KKK KEKE KEK KEKE KKK KKK KE KKK KK KKK KEKE 


# =e>aeea>->-===-==--= Parameters for the PAStrakClacse sass tee eee = 
# the serial port name for the FASTRAK 
PORT: /dev/ttyd2 


Which station do you want to work with? 

A station can set to be active or inactive by the software. Only 
active stations return data. Only the station with its hardware 
switch set on can be set to be active by the software. 

Set the corresponding bit to 1 if you want the station to be active. 
Note that at any time, at least one station must be active. 
WANTED_STATIONS: 1111 


t+ te St + OH + 


# the parameters for the hemisphere and alignment of each station 


These following parameters must be the last part of the file. 

The parameters for a station do not have to be specified here. 

If they are not specified, the default values of the FASTRAK are used. 
The STATION#_PARAM line and the four parameter lines following it must 
immediately follow one another. There can be no comment lines among them. 


+6 te + te OE 


# the hemisphere and alignment of station 1 
STATION1_PARAM: : 
hemisphere: 0 0 
Origin: 0-0 
X_pOdWeE. 07 =1 
VRAofoplulier -1 0 
# the hemisphere and alignment of station 2 
STATION2_PARAM: 
hemisphere: 0 
Origin: 0 
ee Oren te. 0 
We jelonh gee ~1 
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# the hemisphere and alignment of station 3 
STATION3_ PARAM: 


hemisphere: O 0O -1 
Origin. OF G0 
MopeLme : Oo -1 0 
VY epOoInt: =I 0° 0 


# the hemisphere and alignment of station 4 
STATION4_ PARAM: 
hemisphere: 0 0 -1l 


[o@ulic(pivale Oo 0 
SMO OLaC ; O-1 0 
VY point: = 0% “0 
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APPENDIX D: POSITION TRACKING SOFTWARE 


body.h 1 


ie 
(ae 
as 
ef 
me 
cs 
a 
ae 


KEKKKKKKEKKKKEKEKEKEKEKKEKEKKEKEKEKEEKKEKREKKEKEKEKEKKKEKEEKKEKKEKKKKEKKKEKEEKK 


FILENAME: body.h 

PURPOSE: declarations for the Body class 
uses position tracking technique 

AUTHOR: P F Skopowski 

DATE: 1 Aug 96 

COMMENTS: definition of the Body class 


KREEKEKKKKEKKKEKKEKE KK KEKE KEKE KEKE KEK KEKE KEKE KEKE KEKKKKE KK KKKEKK 


#ifndef BODY_H 
#define BODY_H 


#define PF_CPLUSPLUS_API 0 
#include <Performer/pf.h> 
#include “upperbody.h” 
#include “lowerbody.h” 
wonelude ~“FastrakClass.h” 


class Body 


{ 


privece: 
Upperbody upperbody ; 
Lowerbody lowerbody; 


int valid; 
FacstrakGlass *fastrak unit; 


FSTK_ stations torso_sensor; 
FSTK_stations upperarm_sensor; 
FSTK_ stations lowerarm_sensor; 
FSTK Stations hand sensor: 


// Fastrak related coordinate systems 
Pividtwicsnmexelouts, HSEX= CO Uas; H_Cx_to_las, \HEtxstouhs, 


// Calibration matrices 

PEMGERP awn ESlbowlomks, Hts to_link6, H.uas to _jinkZo; 
Etat eemelacetoOulimke. 7m hs to _link24; 
PEVaehinewuees CO DOSTcIG, HW Uas_to_positls; 


// Graphical model related coordinate systems 


piMaerigerdo, 46, H20, H21, H24; 
pPivacniserepositil6é, H positls; 
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// Body part lengths 
float spine_shoulder_length, uarm_length, larm_length, hand_length; 


void outputHMatrix(pfMatrix H_mat); 
public: 

Body (const char *cfg_filename) ; 

~Body () ; 

void rotate (double *); 

void rotate_increment (double *); 

void draw(); 

Void reset () ; 

iteweextsts() { return valid; } 

Votcdegqer all inputs () ; 

int calibrate(); 

int set_joint_angles(); 

Pee cele late. jOrnce angles (double *) ; 

ieseece link lengthn(int, float) ; 


int set_joint_displacement(int, float); 


#endif 
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ee 
// 
// 
// 
Li 
// 
fa 
// 


KKK KR KEKE KEKE KEE KEK KEKE KEKE KK KEK KEKE KKK KEKE KEKE KEK KEK KEKE KKK KEKEKEEK 


FILENAME: body.cc 

PURPOSE: functions for the Body class 
position tracking technique 

AUTHOR: P F Skopowski 

DATE: 1 Aug 96 

COMMENTS: functions for the Body class 


KKEKKKKEKKEKKEKKKEKKEKKEKEKKEKKEKKEKKEKEKEKEKKEKEKKEKKEKEKEKEKEKEKEKKEKKE KK KKEKEEK 


#include <math.h> 
#include <iostream.h> 
fine lude “body .h” 


// 
// 
ae 
ae 
a 


eee mcm mm me ewe ww rw me we we www ew ww we we wie i = 


Function: Body (const char *config_filename) 
Purpose: constructor of the body type 
creates and initializes FastrakClass object 
: uses fastrak.dat configuration file 
Returns: body class object 


es 


Body: :Body (const char *config_filename) 


{ 


valid = FALSE; 
fastrak_ unit = NULL: 


// open configuration file 
ifstream config_fileobj (config_filename) ; 
PE Ween tiaetilecs)) 4 
cerr << “Error: opening configuration fms: “ 
<< config_filename << endl; 
reeurn; 


//- initialize matrices 
pfMakeIdentMat (H_tx_to_ts) ; 
pfMakeIdentMat (H_tx_to_uas) ; 
pfMakeIdentMat (H_tx_to_las) ; 
pfMakeIdentMat (H_tx_to_hs) ; 
pfMakeIdentMat (H_ts_to_link3); 
pfMakeIdentMat (H_ts_to_link6) ; 
pfMakeIdentMat (H_uas_to_1ink20) ; 
pfMakeIdentMat (H_las_to_link21) ; 
pfMakeIdentMat (H_hs_to_link24); 
pfMakeIdentMat (H_ts_to_positl16); 
pfMakeIdentMat (H_uas_to_posit18) ; 
pfMakeIdentMat (H3); 
pfMakeIdentMat (H6) ; 
pfMakeIdentMat (H20) ; 
pfMakeIdentMat (H21) ; 
pfMakeIdentMat (H24) 


? 
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//initialize Fastrak 
fastrak_unit = new FastrakClass(config_fileobj); 


iE 


me we ww www ee ee ee me ei 


(fastrak_unit->exists()) { 


ap 


16 


alge 


td 


(fastrak_unit->getState(FSTK_STATION1) ) 
Gorso sensor = FSTK_STATIONI; 
(fastrak_unit->getState(FSTK_STATION?2) ) 
upperarm_sensor = FSTK_STATION2; 
(fastrak_unit->getState(FSTK_STATION3) ) 
lowerarm_sensor = FSTK_STATIONS3; 
(fastrak_unit->getState(FSTK_STATION4) ) 
hand_sensor = FSTK_STATION4; 


Valia = TRUE; 


// Function: ~Body() 
// Purpose: destructor of the body type 


em ww ww we eee ee i i eee es ee Se 


Body : 


{ 


iE 


Cmastrakeuntts'= NULL) && (fLastrak_unit->exists())) 


:~Body () 


Gelete fastrak_unit; 
fasStaameunit = NULL; 


// Function: rotate (double *angles) 
// Purpose: set upperbody joint angles 


as 


uses the passed in array of values 


void Body::rotate (double *angles) 


{ 


// Function: rotate_increment (double *increment_angles) 


upperbody.rotate(angles) ; 


// Purpose: increment upperbody joint angles 


ff 


void Body: :rotate_increment 


{ 


— eee ewe ew eee eee ee eee we we a i ie i i 


uses the passed in array of values 


upperbody.rotate_increment (increment_angles); 
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(double *increment_angles) 


{ 


body.cc 3 


// Function: draw() 
// Purpose: draw the body in the proper position 


void Body: :draw() 

{ 
glPushMatrix(); 
upperbody.draw(); 
glPopMatrix(); 
lowerbody.draw() ; 


// Function: reset() 
// Purpose: reset upperbody joint angles 


void Body::reset() 
{ 
upperbody.reset (); 


7 runctzon: set. link length(int link, float length) 
// Purpose: set a specified link’s length 


es SMUSeGmeO Giz e Eee litk tO thie user 
// Refurne: “REE if suee@essful 
| iinet tatetateiatatatetatetatatetatatanatatatataatatatatatetatatetetatatatatetateetetiteateteaten 


ame Body::set link lengemmint link, float length) 
{ 
if (upperbody.set_link_length(link, length)) { 
rectumum TRUE; 
} 
return FALSE; 


(7 eeuUnNCe Om: SEE Joint displacement (int link, float length) 
// Purpose: set a specified link’s joint displacement 


af : used to size the link to the user 
77 Returns: TRUE if successful 
/ NS Pe oy a ae as es a ee ee wn eee eee ee ee ae es we ee ee eee 


int Body: :set_joint_displacement(int link, float length) 
{ 
if (upperbody.set_joint_displacement (link, length) ) { 
return TRUE; 
} 
return FALSE; 
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// Function: get_all_inputs () 
// Purpose: get inputs from the fastrak trackers 


LZ : Called to copy latest sample from second buffer 
7. : implemented for double buffering to reduce 

iy : lock overhead 

y7 : Called once at the beginning of each frame 


// Comment: original interface design by Scott McMillan 


void Body::get_all_inputs() 
( 


if (fastrak_unit->exists()) { 
fastrak_unit->copyBuffer(); 


fastrak_unit->getHMatrix(torso_sensor, H_tx_to_ts); 
fastrak_unit->getHMatrix(upperarm_sensor, H_tx_to_uas) ; 
fastrak_unit->getHMatrix(lowerarm_sensor, H_tx_to_las) ; 
fastrak_unit->getHMatrix(hand_sensor, H_tx_to_hs) ; 


77/spuncction: output 
// Purpose: output homogeneous transformation matrix (4x4) 


void Body::outputHMatrix(pfMatrix Hmat) 


{ 
POrmamt t=O 1<4- i+) 
Pmimemoe to.5b to.3b 66.3£ $6.3£\n", 
Poise eipOleeamac( 2) [1], Hmat(i] [2], Hmat[i)][3]); 
prince (ine) 
} 
') Pian aaa a = oS a ee ee ee ee eee 


// Function: set_joint_angles() 
// Purpose: Set the body’s joint angles using fastrak data 
// Returns: TRUE if successful 


int Body::set_joint_angles() 
{ 
imt Valid = FALSE; 


double angles[25]; 


208 


body.cc =) 
for (1a = 0; 15; i++) { 
angles aj =.0..0; 
} 
valid = calculate joint_angles (angles) ; 
if (yates: 
rotate(angles); 


} 


return valid; 


// Punction: calculate_joint_angles(double *) 
// Purpose: calculate inverse kinematics 


age : return the joint angles 

1 > Get _allvinputs must run first to update data 

// Returns: TRUE if successful 

| fmm ee nen n--- 


int Body::calculate_joint_angles(double *angles) 
{ 

int valid = FALSE; 

double thetal- = 0. 

OE 

Oz 


f 


f 


double theta2 
double theta3 
double thetai8 = 
double thetal9 = 
Gouble theta20 = 
double therazl = 
Gouble theta22 = 
Gouble theta23 = 
double theta24 = 


™~e 


0; 
CG; 
Gr 


I 
=e =e 


e =e 


~e 


=a 


Se) Q&S) ©) @& &e& 
QO. eon@ soe rss 


~s 


Gonste double Gdeg eto rad = .017453292519943205. 


if (fastrak_unit->exists()) { 
// must call get_all_inputs() first- 
valid = TRUE; 


// convert reported data using calibration matrices 
pDiMultMat(H3, H_tx_to_ts, H_ts_to_link3) ;//using@@alibmmacrix 


/(oiveemiemdets cesired = ({ 0.0, 0.0, =1.0, Ce0RF 

Tif { 0.0, =1.0; OpGRO-O); 

// f=. OF 02.0, 070. Con 

Ho { 0.0, 0.0, O20; 9140) ) moe ea meme 
//pfCopyMat (H3, H_ts_desired);//not using calib 


pfMultMat (H20, H_tx_to_uas, H_uas_to_]link20); //using calib matrix 
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6 
ees keke tee SVE StL OGale Oorreece ERacking~ << = eee ee xs 2 ee ee ee 
eetatrix H3_inv; 
DiMaltMaciH6, Hotx to ts, Hots to _link6);//using calib matrix 
pf£MultMat (H_posit16, H_tx_to_ts, H_ts_to_positl16); 
pfMultMat (H_posit18, H_tx_to_uas, H_uas_to_posit18); 
pfiMatrix templ, temp2; 
pfMakeIdentMat(temp1); 
pfMakeIdentMat (temp2) ; 
Penmaes = Hopositle(0] [3] - H-positié[(0)(3]; 


tempi (2) [3] Pepocters (il i(3) = Heposi1616(1) [3]; 
temp1 [2] [3] Hepestrels [2))(3)-— BHepesitl6(2) [3]; 


// now convert the vector to a torso coord system 
pfMatrix R6, R6_inv; 

ptfMakeIdentMat (R6) ; 

DpEGepyMat(R6, H6); 

Preseemacecoliko, 3, 020, 0.0, 0.0, 1.0); 
pfTransposeMat(R6_inv, R6); 

pfMultMat(temp2, R6_inv, templ); 


Pigatmenerals = atan2 (temp2 (2) [3], temp2(0] [3]); 
iiveemecale <—10.0) { 

thetal6é += 6.283185307; 
} 


float length = sqrt(temp2[0])[3] * temp2[0][3] + 
Eemp2 (203) = ctCemp2 12) (32)3- 
float thetal7 = atan2(-temp2([(1)] [3], length); 


[fr RR ERERRREKREERRREAEKEKKEKKEEKEREREEKEEEKEKEKEKERERKEKEKKEKKERKEKER 


peMulbtMat{H21, H tx to las, H_las_ to link21); 
Dr leviatth24e nets to ns, H hs to Jink24); 


// get the data from H3 
Ggeuble a3 = H3(2] [0]; 
double b3 = H3(2] (1); 
double c3 He 2 2 i 
double c2 Mot 1). [204 
double cl = H3[0] [2]; 


// compute the sin of theta2 
Gaeujlesitwenetad =“sqrticl * ¢1 + ¢c2 * ¢2): 
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7 
// eneeck for zero 
Lie eeime@enctal =< —0. 001) { 
Simeeieczaz =o. 001 ; 
} 
// set the sign of the answer 
if wie = 0.0)4 
Sin _theta2 *= -1.0; 
} 
// compute the angles 
theta2 = atan2(sin_theta2, -c3); 
theta3 = atan2(-b3/sin_theta2, a3/sin_theta2); 
thetal = atan2(c2/sin_theta2, cl/sin_thetaz2) ; 
// compute T_17_to_20 
pirMatrix T_17_to_20, H_temp; 
peice Toi? seers = 1{ O20, -1,0, “O.0;, 0.753, 
(ule Ose 0. 05. §O.0,- 25 Cae 
{en eeeOsO, 1.07. O20 
{EO Oye “0.0; Om0,. IeGae 


pfinvertFullMat (H3_inv, H3); 


pfMultMat (H_temp, H3_inv, H20); 
pavuteMar() l7eco 20, T 17 -to.3, HB temp; 


// get the data from T_17_to_20 

Geupleta2 =. l iymror Oia Cl: 

Gdeouple p72 = Tel7lteuzo (1) fli 
Seo lel meou 2 Ol | (2): 
e2 ioe Oo 2 O(a (2 1 
Gal Paleo. © 10] (2): 


// compute the sin of thetai9 
GoublevVeaineenetalo = Sartta2? * az + b2 *7p2)- 


/7/ Cheek for zero 
if (Sin _theeal9 < 0.001) { 
Sim thecats = 0.001; 


// set the sign of the answer 
iE) (edge 0..0 ) 4 
Sin _theta2 *= -1.0; 


// compute the angles 


theta1l9 = atan2(sin_thetal9, c2); 
theta20 = atan2(b2/sin_thetal9, -a2/sin_thetal9); 
thetal8 = atan2(c3/sin_thetal9, cl/sin_thetal9) ; 
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J// Gemoute T20to_21 
Deviactrix To202b6.21, H20_ inv; 


BiimvercerullMat (H20 inv, H20) ; 
PeoleMac(T 20 to 21, H20_inv, H21); 
Vyeeerycne Gata Erom T_20_to_21 


age ee eo 21. (2) (0); 
Jes) meetOeco.21 (2) (1); 


// compute the angle 
theta21 = atan2(a3, b3); 


DiMe@trix T_21 to.24, H21_inv; 
BeloverccruliMat(H21_inv, 21); 
DuiimeMatite2l to 24, H21_inv, H24); 


// get the data from H24 
aoe eon {21 (0) ; 


Big= tel to. 24 (2) (1); 
Cet? leon 4122): 
Coe teto 24 [1 ) [2]: 
Cle eres leteO. 240)).( 2)'s 


//*eompuire the sin of theta23 
Seupvenstineemecass = -Ssert(a3 * a3 + b3 * b3); 


7 eGneck for Zero 

Heeiein theta23; => -0.001) { 
—Seectm cneta2s = -0.001; 

} 


// compute the angles 

theta23 = atan2(sin_theta23, -c3); 

theta24 atan2 (b3/sin_theta23, -a3/sin_theta23) ; 
theta22 atean2 (=c2/sin_theta23, -cl/sin_theta23); 


// convert ali angles to degrees 
thetal /= deg_to_rad; 
Emetua2 /= deg _to rad: 
theca3s /= deg _to_rad; 
Ghetals (= deg to. rad; 
thetal7 /= deg_to_rad; 
Emecates/= Ceg to rad;..., 
Ghecaly -/= deg_to rad; 
Gpeta20.7 = ceq to rad; 
thecal /= deg to.rad; 
EmecaZ2 /= deg_to rad; 
bneGcags /= deg to_rad:; 
theta24 /= deg_to_rad; 


Z2 
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anglesimar = thetal; 
angues(?2|. =_@Enetad ; 
angles[3}] = theta3; 
angles[4]) = S020: 
angles[5]) = 9020; 
angles[6) = 180.0; 
angles[7]) = OG. 
angles[8]) = oro; 
angles[9]) = 30.0; 
angles[10] = 90.0; 
angles[1l1] = O05 
angles[12]) = 0-0" 
angles[13) = O...07 
angles[14) = -90.0; 
angres (is >= Om0; 
angles[16]) = thetalé6; 
angles[17] = thetal7; 
angles[18] = 90.0; 
angles([i9) = 90.0; 
angles[20] = 0.0; 
angles[21) = OF 0: 
angles[22) = 0.0; 
angles[23]) = -90.0; 
angles[24) = C20: 


} 


rehurn valida: 


V7 FUNCEION: Calibrate () 
// Purpose: size the upperbody model to the user 


// >: Calibrate the trackers 

// Returns: TRUE if successful 

a 
int Body::calibrate() 

{ 


int valid = FALSE; 


pfMatrix H_torso_reported, H_uarm_reported; 
pfMatrix H_larm_reported, H_hand_reported; 


pfMakeIdentMat 
pfMakeIdentMat 
pfMakeIdentMat 
pfMakeIdentMat 


Heeorso reported) ; 
H_uarm_reported) ; 
H_larm_reported) ; 
H_hand_reported) ; 


ee ee eee 


if (fastrak_unit->exists()) { 
Valia = TRUE; 
eharssin: 


Ze 
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cerr << endl << “Calibrating sensor orientation in 3 seconds...” << endl; 
cerr << “Press <Enter> to start count-down: ”; 
CMiade ccc ); 
Pee int J=O7ei<3; i++) { 
sleep(1); 
Gerry << (char) 7; 


V7 mene coder allows the fastrak to do the calibration for torso 
//float angles[3] = (90.0, -90.0, 180.0}; 
//fastrak_unit->setBoresight (torso_sensor, angles); 


// get the data to compute the calibration matrices 
fastrak_unit->copyBuffer(); 
fastrak_unit->getHMatrix(torso_sensor, H_torso_reported) ; 
fastrak_unit->getHMatrix(upperarm_sensor, H_uarm_reported) ; 
fastrak_unit->getHMatrix(lowerarm_sensor, H_larm_reported) ; 
fastrak_unit->getHMatrix(hand_sensor, H_hand_reported) ; 


/pmeetpuce Ene Calibration matrices 


/7/ Gompuce Corso sensor calibration matrix 
pfMatrix H_torso_reported_inv; 


PLvdeeme Herswaesirea = ({ 0.0, 0.0, -1.0, 0.0}, 
Pico = 120, CGO}, 
Cee O07. <O020, 0:0), 
fe On Oe OrOs 6 6 a bere 0 ie 


pfinvertFullMat (H_torso_reported_inv, H_torso_reported) ; 


Petwmemacit ts to links, H terso_reported_inv, H_ts_desired) ; 


MPedemwicetoss G@esired? = {{ 0.0, 0.0, -1.0, 0.0}, 
ti. 0) OO, Oreo er O) 7 
WOO. oO OeO, OO}, 
fp. «Oia, lees] O Baad Sea 6 i oan 


pfMultMat (H_ts_to_link6é, H_torso_reported_inv, H_ts_desiredz2) ; 
re TA -ivestigqate Ofteet Gisackane. 3. <4 <2 es Skee x AERA EX 
po meompuce *temso Sensor calibration matrix for positi6 tracking 
// some necessary matrices 

DEiMaem@ioe Retswmeo ts. Rk ts to tx: 


pfCopyMat (R_tx_to_ts, H_torso_reported) ; 


jo eSeGePOslre GOl "GG zero CO work with rotation matrix only 
Pee ace Oltnmex to ts, 3, 0.0, 0.0, 0.0, 1.0); 


214 


body.cc 11 


// get the inverse rotation matrix 
Dt lLeameneseMaei kets to tx, REx _to_ts); 


// determine suitable offsets from ts and uas position data 
flog otksee = Hua reported(0](3] - H_torso_reported(0)] [3]; 
float y_offset eeal@e 

float z_offset DAO. 


/(wEnewOetScecee trem Certo relay in world coordinates 


DiMa@rix@heOmsset (es to _relav = {{ 1.0, 0.0, 0.0, X_OffSet}), 
{O70 Oe Cow y_offset}, 
GeO, \Oeiirre IO. 7 O©Lf Seely 
{ 0.0, Smee 0 3e7 1035)5 


pfMatrix temp; 
pfiMultMat (temp, R_ts_to_tx, P_offset_ts_to_rclav) ; 
DESeCEMaECOI(H tsetenpesitis, 3, "tempi0] (3), templi)iai, temo ieee 


// compute upper arm sensor calibration matrix 
pfMatrix H_uarm_reported_inv; 


pDiMat.iketawemmeesired = (i20-Orem0.G> -1.0;7 O20}, 
moro, 1.0, S@Ororeonoy, 
forleeOy oO, On OpeOmies 
(iad 0 OF Os TOs 


pfInvertFullMat (H_uarm_reported_inv, H_uarm_reported) ; 
pfMultMat (H_uas_to_link20, H_uarm_reported_inv, H_uarm_desired) ; 
// compute upper arm sensor calibration matrix for positl8 tracking 


// some necessary matrices 
DiMatrixeheex to uas, R wes to tx; 


pfCopyMat (R_tx_to_uas, H_uarm_reported); 


/7 set posit col to zero to work with rotation macrix sem, 
DESCEMateol (Retx to-ues y 3, 0.0, 0-0720-0, eu 


// get the inverse rotation matrix - 
pfTransposeMat (R_uas_to_tx, R_tx_to_uas) ; 


// determine suitable offsets from uas and ts position data 


MTOLiSete=-, 0.0; 

y_offset = - (fabs(H_torso_reported[(1][3] - H_uarm_reported[1][3]) - 
m2. 0) 

Z@ Offset = = fabs(H torso _reported([2] [3] - H_uarm reperted(o iii 
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12 


/7/ the offset from uas to shoulder in world coordinates 
wae, O.0, 0.0, 


pfMatrix P_offset_uas_to_shoulder 


ics, 
O70 
{ORC 


f , 


20 
noe 
a0 


a 


/ 


oO 
Sore 
Qi & 


é f 


DeuleMac (tenp, Ro Was to tx, P_offset_uas to shoulder); 


pfSetMatCol (H_uas_to_positl18, 3, 


temp{0]{3}, temp{[1]{3], 


MSOfESEt}, 
y_offset}, 
Z-O0ffset}, 
1.0}); 


bempilz)12)7 


[ERR RE ERRERREREKREKERREREREEKEEREKKEKKEERERKEKKKEKKKKRKERKEKKEKKEKK KEK 


// compute lower arm sensor calibration matrix 


pfMatrix H_larm_reported_inv; 


pfMatrix H_larm_desired = { 


= 


Q@) [SQ] © 
e . e * 
2) (2) (i (Se 


{ 
{ 
{ 
{ 


ui 


2) @) & [= 


pfInvertFullMat (H_larm_reported_inv, 


PrvuUbenae (Hh bas to, link21, H larm_reported_inv, 


Oi OO 580.0), 
CaO 0%, 
OF 0-0 4020), 
Oy. 0.07, 1.0)); 


H_larm_reported) ; 


// compute hand sensor calibration matrix 


pfMatrix H_hand_reported_inv; 


pfMatrix H_hand_desired = { 


QQ &] - 


DEwmvyecreruliMat (H hand reported_inv, 


pfMultMat (H_hs_to_link24, H_hand_reported_inv, 


On. 0, 0.0), 
OC; Vomorae. 0}, 
Cee 1. 0,020 }:, 
Oj Uren OF} ) 7 


H_hand_reported) ; 


// set upperbody dimensions to that of the user 


eeuelink lengtih(3; 28.0); 
Seeolink length (6, 8.0); 
ScesCintealsplacement (16; 26.0); 
Seemiankelengtn (17, 14.0); 
See inks length (8. 14.0); 
SCrre Mik henge (2G, 20...0) > 
Seem Minka vength (ia, 28.0): 
Sctamkmrengthi( 21. 29.0) ; 
set_link lengen(12, 29.0); 
SGeeltmkelengtn(24, 17.0); 
Sep. tance vencen(ls, 17.0): 
} 


return valid; 
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BPelarm Gesired) ; 


H_hand_desired); 


Ale 


Oe 


APPENDIX E: DEMONSTRATION VIDEO 


(SEE ACCOMPANYING VHS VIDEO TAPE: 
“Immersive Articulation of the Human Upper Body 
in a Virtual Environment 
Appendix E: Demonstration Video” ) 
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